From 22d46fa73383014172fd0a0df65906683df3be51 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 9 Jul 2015 11:22:35 -1000 Subject: [PATCH 001/136] Merged master_uavcan_modular src/modules/uavcan/ --- src/lib/uavcan | 2 +- src/modules/uavcan/actuators/esc.cpp | 34 +- src/modules/uavcan/actuators/esc.hpp | 8 +- src/modules/uavcan/module.mk | 7 +- src/modules/uavcan/sensors/gnss.cpp | 28 +- src/modules/uavcan/sensors/mag.cpp | 57 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 9 +- src/modules/uavcan/sensors/sensor_bridge.hpp | 21 +- src/modules/uavcan/uavcan_main.cpp | 324 +++++++---- src/modules/uavcan/uavcan_main.hpp | 39 +- src/modules/uavcan/uavcan_servers.cpp | 280 ++++++++++ src/modules/uavcan/uavcan_servers.hpp | 145 +++++ .../uavcan/uavcan_virtual_can_driver.hpp | 506 ++++++++++++++++++ 13 files changed, 1258 insertions(+), 202 deletions(-) create mode 100644 src/modules/uavcan/uavcan_servers.cpp create mode 100644 src/modules/uavcan/uavcan_servers.hpp create mode 100644 src/modules/uavcan/uavcan_virtual_can_driver.hpp diff --git a/src/lib/uavcan b/src/lib/uavcan index 1f1679c75d..4d18f381e5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 1f1679c75d989420350e69339d48b53203c5e874 +Subproject commit 4d18f381e580dc0e086a8fb69469fa28e88244bc diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index c6719d4817..51ee776a97 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -68,8 +68,8 @@ int UavcanEscController::init() { // ESC status subscription int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("ESC status sub failed %i", res); return res; } @@ -83,9 +83,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || - (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || - (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Rate limiting - we don't want to congest the bus */ const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } + _prev_cmd_pub = timestamp; /* @@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - // trim negative values back to 0. Previously - // we set this to 0.1, which meant motors kept - // spinning when armed, but that should be a - // policy decision for a specific vehicle - // type, as it is not appropriate for all - // types of vehicles (eg. fixed wing). + + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). if (scaled < 0.0F) { scaled = 0.0F; - } + } + if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); @@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); + } else { msg.cmd.push_back(static_cast(0)); } @@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm) { if (arm) { _armed_mask = -1; + } else { _armed_mask = 0; } @@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm) { if (arm) { _armed_mask = MOTOR_BIT(num); + } else { _armed_mask = 0; } @@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< } } -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) { _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; if (_esc_status_pub > 0) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c0355422..67bb47cc14 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -54,7 +54,7 @@ class UavcanEscController { public: - UavcanEscController(uavcan::INode& node); + UavcanEscController(uavcan::INode &node); ~UavcanEscController(); int init(); @@ -79,12 +79,12 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; - typedef uavcan::MethodBinder&)> StatusCbBinder; - typedef uavcan::MethodBinder - TimerCbBinder; + typedef uavcan::MethodBinder + TimerCbBinder; bool _armed = false; esc_status_s _esc_status = {}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 1d6c205586..da62e55634 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ + uavcan_servers.cpp \ uavcan_clock.cpp \ uavcan_params.c @@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # 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 +override EXTRADEFINES := $(EXTRADEFINES) \ +-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ +-DUAVCAN_NO_ASSERTIONS \ +-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 3ae07367fa..52e4abbaf1 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -46,20 +46,21 @@ const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : -_node(node), -_sub_fix(node), -_report_pub(-1) + _node(node), + _sub_fix(node), + _report_pub(-1) { } int UavcanGnssBridge::init() { int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); - if (res < 0) - { + + if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; } @@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const void UavcanGnssBridge::print_status() const { printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { printf("N/A\n"); + } else { printf("%d\n", _receiver_node_id); } @@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { report.eph = -1.0F; report.epv = -1.0F; } if (valid_velocity_covariance) { - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. @@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure(arg), sizeof(_scale)); - return 0; - } + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { - std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); - return 0; - } + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { - return 0; // Nothing to do - } + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { - return 1; // declare it external rise it's priority and to allow for correct orientation compensation - } + return 1; // declare it external rise it's priority and to allow for correct orientation compensation + } + case MAGIOCSSAMPLERATE: { - return 0; // Pretend that this stuff is supported to keep the sensor app happy - } + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: case MAGIOCGSAMPLERATE: case MAGIOCSRANGE: @@ -121,15 +132,17 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar case MAGIOCSLOWPASS: case MAGIOCEXSTRAP: case MAGIOCGLOWPASS: { - return -EINVAL; - } + return -EINVAL; + } + default: { - return CDev::ioctl(filp, cmd, arg); - } + return CDev::ioctl(filp, cmd, arg); + } } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure + &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index b370764440..e19235e525 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,7 +45,7 @@ /* * IUavcanSensorBridge */ -void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { list.add(new UavcanBarometerBridge(node)); list.add(new UavcanMagnetometerBridge(node)); @@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } + delete [] _channels; } @@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { _out_of_channels = true; log("out of class instances"); @@ -119,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) channel->class_instance = class_instance; channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); + if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -128,6 +131,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } + assert(channel != nullptr); (void)orb_publish(_orb_topic, channel->orb_advert, report); @@ -136,11 +140,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { if (_channels[i].node_id >= 0) { out += 1; } } + return out; } @@ -152,6 +158,7 @@ void UavcanCDevSensorBridgeBase::print_status() const if (_channels[i].node_id >= 0) { printf("channel %d: node id %d --> class instance %d\n", i, _channels[i].node_id, _channels[i].class_instance); + } else { printf("channel %d: empty\n", i); } diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index de130b0788..dc2a1983b0 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -45,7 +45,7 @@ /** * A sensor bridge class must implement this interface. */ -class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: static constexpr unsigned MAX_NAME_LEN = 20; @@ -77,7 +77,7 @@ public: * Sensor bridge factory. * Creates all known sensor bridges and puts them in the linked list. */ - static void make_all(uavcan::INode &node, List &list); + static void make_all(uavcan::INode &node, List &list); }; /** @@ -86,8 +86,7 @@ public: */ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev { - struct Channel - { + struct Channel { int node_id = -1; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -104,13 +103,13 @@ protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor, - const unsigned max_channels = DEFAULT_MAX_CHANNELS) : - device::CDev(name, devname), - _max_channels(max_channels), - _class_devname(class_devname), - _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]) + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : + device::CDev(name, devname), + _max_channels(max_channels), + _class_devname(class_devname), + _orb_topic(orb_topic_sensor), + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f4763fce7f..94261ac9cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,15 +53,12 @@ #include #include "uavcan_main.hpp" +#include -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 -#endif + /** * @file uavcan_main.cpp @@ -76,33 +73,28 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; -#if defined(USE_FW_NODE_SERVER) -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; -#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), -#if !defined(USE_FW_NODE_SERVER) _esc_controller(_node) -#else - _esc_controller(_node), - _fileserver_backend(_node), - _node_info_retriever(_node), - _fw_upgrade_trigger(_node, fw_version_checker), - _fw_server(_node, _fileserver_backend) -#endif - { + _task_should_exit = false; + _fw_server_action = None; + _fw_server_status = -1; + _tx_injector = nullptr; _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - const int res = pthread_mutex_init(&_node_mutex, nullptr); + int res = pthread_mutex_init(&_node_mutex, nullptr); + + if (res < 0) { + std::abort(); + } + + res = sem_init(&_server_command_sem, 0 , 0); if (res < 0) { std::abort(); @@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys UavcanNode::~UavcanNode() { + + fw_server(Stop); + if (_task != -1) { + /* tell the task we want it to go away */ _task_should_exit = true; @@ -161,13 +157,127 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - -#if defined(USE_FW_NODE_SERVER) - delete(_server_instance); -#endif + pthread_mutex_destroy(&_node_mutex); + sem_destroy(&_server_command_sem); } +int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) +{ + int rv = -1; + + if (UavcanNode::instance()) { + 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 + } + + uint8_t udid[12] = {}; // Someone seems to love magic numbers + get_board_serial(udid); + uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); + rv = 0; + } + + return rv; +} + +int UavcanNode::start_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers == nullptr) { + + rv = UavcanServers::start(2, _node); + + if (rv >= 0) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().installRxFrameListener(driver.get()); + * would require a dynamic cast and rtti is not enabled. + */ + UavcanServers::instance()->attachITxQueueInjector(&_tx_injector); + } + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + +int UavcanNode::request_fw_check() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + _servers->requestCheckAllNodesFirmwareAndUpdate(); + rv = 0; + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; + +} + +int UavcanNode::stop_fw_server() +{ + int rv = -1; + _fw_server_action = Busy; + UavcanServers *_servers = UavcanServers::instance(); + + if (_servers != nullptr) { + /* + * Set our pointer to to the injector + * This is a work around as + * main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + _tx_injector = nullptr; + + rv = _servers->stop(); + } + + _fw_server_action = None; + sem_post(&_server_command_sem); + return rv; +} + + +int UavcanNode::fw_server(eServerAction action) +{ + int rv = -EAGAIN; + + switch (action) { + case Start: + case Stop: + case CheckFW: + if (_fw_server_action == None) { + _fw_server_action = action; + sem_wait(&_server_command_sem); + rv = _fw_server_status; + } + + break; + + default: + rv = -EINVAL; + break; + } + + return rv; +} + + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; + } + const int node_init_res = _instance->init(node_id); if (node_init_res < 0) { @@ -248,7 +363,7 @@ void UavcanNode::fill_node_info() assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); - swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT; + swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); @@ -256,21 +371,7 @@ void UavcanNode::fill_node_info() /* hardware version */ uavcan::protocol::HardwareVersion hwver; - - 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 - } - - uint8_t udid[12] = {}; // Someone seems to love magic numbers - get_board_serial(udid); - uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin()); - + getHardwareVersion(hwver); _node.setHardwareVersion(hwver); } @@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } -#if defined(USE_FW_NODE_SERVER) - /* 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; - } - -#endif /* Start the Node */ return _node.start(); @@ -398,6 +431,11 @@ void UavcanNode::node_spin_once() warnx("node spin error %i", spin_res); } + + if (_tx_injector != nullptr) { + _tx_injector->injectTxFramesInto(_node); + } + perf_end(_perfcnt_node_spin_elapsed); } @@ -446,7 +484,7 @@ int UavcanNode::run() * IO multiplexing shall be done here. */ - _node.setStatusOk(); + _node.setModeOperational(); /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). @@ -466,6 +504,24 @@ int UavcanNode::run() while (!_task_should_exit) { + switch (_fw_server_action) { + case Start: + _fw_server_status = start_fw_server(); + break; + + case Stop: + _fw_server_status = stop_fw_server(); + break; + + case CheckFW: + _fw_server_status = request_fw_check(); + break; + + case None: + default: + break; + } + // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { subscribe(); @@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { + sem_post(&_server_command_sem); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); @@ -812,7 +870,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop|arm|disarm}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + bool fw = argc > 2 && !std::strcmp(argv[2], "fw"); + if (!std::strcmp(argv[1], "start")) { if (UavcanNode::instance()) { + if (fw && UavcanServers::instance() == nullptr) { + int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); + + if (rv < 0) { + warnx("Firmware Server Failed to Start %d", rv); + ::exit(rv); + } + + ::exit(0); + } + // Already running, no error warnx("already started"); ::exit(0); @@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[]) errx(1, "application not running"); } + if (fw && !std::strcmp(argv[1], "update")) { + if (UavcanServers::instance() == nullptr) { + errx(1, "firmware server is not running"); + } + + int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW); + ::exit(rv); + } + + if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) { + printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped"); + ::exit(0); + } + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); ::exit(0); @@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + if (fw) { + + int rv = inst->fw_server(UavcanNode::Stop); + + if (rv < 0) { + warnx("Firmware Server Failed to Stop %d", rv); + ::exit(rv); + } + + ::exit(0); + + } else { + delete inst; + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 43d82082b4..3413f31bf1 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -48,13 +48,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -#if defined(USE_FW_NODE_SERVER) -# include -# include -# include -# include -# include -#endif +# include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -66,9 +60,6 @@ #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) @@ -84,8 +75,7 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why - + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -99,11 +89,12 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 3400; + static constexpr unsigned StackSize = 1600; public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; + enum eServerAction {None, Start, Stop, CheckFW , Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); @@ -126,6 +117,9 @@ public: void print_info(); static UavcanNode *instance() { return _instance; } + static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); + int fw_server(eServerAction action); + void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} private: void fill_node_info(); @@ -133,10 +127,14 @@ private: void node_spin_once(); int run(); int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] - + int start_fw_server(); + int stop_fw_server(); + int request_fw_check(); int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + volatile eServerAction _fw_server_action; + int _fw_server_status; int _armed_sub = -1; ///< uORB subscription of the arming status actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus @@ -151,22 +149,13 @@ private: Node _node; ///< library instance pthread_mutex_t _node_mutex; - + sem_t _server_command_sem; UavcanEscController _esc_controller; - -#if defined(USE_FW_NODE_SERVER) - static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer - - uavcan_posix::BasicFileSeverBackend _fileserver_backend; - uavcan::NodeInfoRetriever _node_info_retriever; - uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; - uavcan::BasicFileServer _fw_server; -#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; - + ITxQueueInjector *_tx_injector; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp new file mode 100644 index 0000000000..6c4d70e3cd --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uavcan_main.hpp" +#include "uavcan_servers.hpp" +#include "uavcan_virtual_can_driver.hpp" + +# include +# include +# include + +//todo:The Inclusion of file_server_backend is killing +// #include and leaving OK undefined +# define OK 0 + +/** + * @file uavcan_servers.cpp + * + * Implements basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * David Sidrane + */ + +/* + * UavcanNode + */ +UavcanServers *UavcanServers::_instance; +UavcanServers::UavcanServers(uavcan::INode &main_node) : + _subnode_thread(-1), + _vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _main_node(main_node), + _tracer(), + _storage_backend(), + _fw_version_checker(), + _server_instance(_subnode, _storage_backend, _tracer), + _fileserver_backend(_subnode), + _node_info_retriever(_subnode), + _fw_upgrade_trigger(_subnode, _fw_version_checker), + _fw_server(_subnode, _fileserver_backend), + _mutex_inited(false), + _check_fw(false) + +{ +} + +UavcanServers::~UavcanServers() +{ + if (_mutex_inited) { + (void)Lock::deinit(_subnode_mutex); + } +} + +int UavcanServers::stop(void) +{ + UavcanServers *server = instance(); + + if (server == nullptr) { + warnx("Already stopped"); + return -1; + } + + _instance = nullptr; + + if (server->_subnode_thread != -1) { + pthread_cancel(server->_subnode_thread); + pthread_join(server->_subnode_thread, NULL); + } + + server->_main_node.getDispatcher().removeRxFrameListener(); + + delete server; + return 0; +} + +int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) +{ + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } + + /* + * Node init + */ + _instance = new UavcanServers(main_node); + + if (_instance == nullptr) { + warnx("Out of memory"); + return -2; + } + + int rv = _instance->init(num_ifaces); + + if (rv < 0) { + warnx("Node init failed %i", rv); + delete _instance; + return rv; + } + + /* + * Start the thread. Normally it should never exit. + */ + pthread_attr_t tattr; + struct sched_param param; + + pthread_attr_init(&tattr); + tattr.stacksize = StackSize; + param.sched_priority = Priority; + pthread_attr_setschedparam(&tattr, ¶m); + + static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; + + rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); + + if (rv < 0) { + warnx("start failed: %d", errno); + rv = -errno; + delete _instance; + } + + return rv; +} + +int UavcanServers::init(unsigned num_ifaces) +{ + /* + * Initialize the mutex. + * giving it its path + */ + + int ret = Lock::init(_subnode_mutex); + + if (ret < 0) { + return ret; + } + + _mutex_inited = true; + + _subnode.setNodeID(_main_node.getNodeID()); + _main_node.getDispatcher().installRxFrameListener(&_vdriver); + + + /* + * Initialize the fw version checker. + * giving it its 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; + } + + /* hardware version */ + uavcan::protocol::HardwareVersion hwver; + UavcanNode::getHardwareVersion(hwver); + + /* Initialize the dynamic node id server */ + ret = _server_instance.init(hwver.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 OK; +} +__attribute__((optimize("-O0"))) +pthread_addr_t UavcanServers::run(pthread_addr_t) +{ + Lock lock(_subnode_mutex); + + while (1) { + + if (_check_fw == true) { + _check_fw = false; + _node_info_retriever.invalidateAll(); + } + + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } + } + + warnx("exiting."); + return (pthread_addr_t) 0; +} + diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp new file mode 100644 index 0000000000..9a68a8cc25 --- /dev/null +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +#include +#include + +# include +# include +# include +# include +# include +# include +# include +# include + +# include "uavcan_virtual_can_driver.hpp" + +/** + * @file uavcan_servers.hpp + * + * Defines basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * @author David Sidrane + */ + +#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" + +/** + * A UAVCAN Server Sub node. + */ +class UavcanServers +{ + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; + + static constexpr unsigned MaxCanFramsPerTransfer = 63; + + /** + * This number is based on the worst case max number of frames per interface. With + * MemPoolBlockSize set at 48 this is 6048 Bytes. + * + * The servers can be forced to use the primary interface only, this can be achieved simply by passing + * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. + */ + static constexpr unsigned QueuePoolSize = + (UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); + + static constexpr unsigned StackSize = 3500; + static constexpr unsigned Priority = 120; + + typedef uavcan::SubNode SubNode; + +public: + UavcanServers(uavcan::INode &main_node); + + virtual ~UavcanServers(); + + static int start(unsigned num_ifaces, uavcan::INode &main_node); + static int stop(void); + + SubNode &get_node() { return _subnode; } + + static UavcanServers *instance() { return _instance; } + + /* + * Set the main node's pointer to to the injector + * This is a work around as main_node.getDispatcher().remeveRxFrameListener(); + * would require a dynamic cast and rtti is not enabled. + */ + void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;} + + void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; } + +private: + pthread_t _subnode_thread; + pthread_mutex_t _subnode_mutex; + + int init(unsigned num_ifaces); + void deinit(void); + + pthread_addr_t run(pthread_addr_t); + + static UavcanServers *_instance; ///< singleton pointer + + typedef VirtualCanDriver vCanDriver; + + vCanDriver _vdriver; + + uavcan::SubNode _subnode; ///< library instance + uavcan::INode &_main_node; ///< library instance + + uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; + uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; + uavcan_posix::FirmwareVersionChecker _fw_version_checker; + uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer + uavcan_posix::BasicFileSeverBackend _fileserver_backend; + uavcan::NodeInfoRetriever _node_info_retriever; + uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; + uavcan::BasicFileServer _fw_server; + + bool _mutex_inited; + volatile bool _check_fw; + +}; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp new file mode 100644 index 0000000000..7a151d2cd2 --- /dev/null +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* + * General purpose wrapper around os's mutual exclusion + * mechanism. + * + * It supports the + * + * Note the naming of thier_mutex_ implies that the underlying + * mutex is owned by class using the Lock. + * and this wrapper provides service to initialize and de initialize + * the mutex. + */ +class Lock +{ + pthread_mutex_t &thier_mutex_; + +public: + + Lock(pthread_mutex_t &m) : + thier_mutex_(m) + { + (void)pthread_mutex_lock(&m); + } + + ~Lock() + { + (void)pthread_mutex_unlock(&thier_mutex_); + } + + static int init(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_init(&thier_mutex_, NULL); + } + + static int deinit(pthread_mutex_t &thier_mutex_) + { + return pthread_mutex_destroy(&thier_mutex_); + } + +}; + +/** + * Generic queue based on the linked list class defined in libuavcan. + * This class does not use heap memory. + */ +template +class Queue +{ + struct Item : public uavcan::LinkedListNode { + T payload; + + template + Item(Args... args) : payload(args...) { } + }; + + uavcan::LimitedPoolAllocator allocator_; + uavcan::LinkedListRoot list_; + +public: + Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : + allocator_(arg_allocator, block_allocation_quota) + { + uavcan::IsDynamicallyAllocatable::check(); + } + + bool isEmpty() const { return list_.isEmpty(); } + + /** + * Creates one item in-place at the end of the list. + * Returns true if the item was appended successfully, false if there's not enough memory. + * Complexity is O(N) where N is queue length. + */ + template + bool tryEmplace(Args... args) + { + // Allocating memory + void *const ptr = allocator_.allocate(sizeof(Item)); + + if (ptr == nullptr) { + return false; + } + + // Constructing the new item + Item *const item = new(ptr) Item(args...); + assert(item != nullptr); + + // Inserting the new item at the end of the list + Item *p = list_.get(); + + if (p == nullptr) { + list_.insert(item); + + } else { + while (p->getNextListNode() != nullptr) { + p = p->getNextListNode(); + } + + assert(p->getNextListNode() == nullptr); + p->setNextListNode(item); + assert(p->getNextListNode()->getNextListNode() == nullptr); + } + + return true; + } + + /** + * Accesses the first element. + * Nullptr will be returned if the queue is empty. + * Complexity is O(1). + */ + T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; } + const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; } + + /** + * Removes the first element. + * If the queue is empty, nothing will be done and assertion failure will be triggered. + * Complexity is O(1). + */ + void pop() + { + Item *const item = list_.get(); + assert(item != nullptr); + + if (item != nullptr) { + list_.remove(item); + item->~Item(); + allocator_.deallocate(item); + } + } +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + */ +class VirtualCanIface : public uavcan::ICanIface, + uavcan::Noncopyable +{ + /** + * This class re-defines uavcan::RxCanFrame with flags. + * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded, + * rendering this class unusable with Queue<>. + */ + struct RxItem: public uavcan::CanFrame { + const uavcan::MonotonicTime ts_mono; + const uavcan::UtcTime ts_utc; + const uavcan::CanIOFlags flags; + const uint8_t iface_index; + + RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) : + uavcan::CanFrame(arg_frame), + ts_mono(arg_frame.ts_mono), + ts_utc(arg_frame.ts_utc), + flags(arg_flags), + iface_index(arg_frame.iface_index) + { + // Making sure it will fit into a pool block with a pointer prefix + static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee"); + } + }; + + pthread_mutex_t &common_driver_mutex_; + + uavcan::CanTxQueue prioritized_tx_queue_; + Queue rx_queue_; + + int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override + { + Lock lock(common_driver_mutex_); + prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags); + return 1; + } + + int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override + { + Lock lock(common_driver_mutex_); + + if (rx_queue_.isEmpty()) { + return 0; + } + + const auto item = *rx_queue_.peek(); + rx_queue_.pop(); + + out_frame = item; + out_ts_monotonic = item.ts_mono; + out_ts_utc = item.ts_utc; + out_flags = item.flags; + + return 1; + } + + int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; } + uint16_t getNumFilters() const override { return 0; } + uint64_t getErrorCount() const override { return 0; } + +public: + VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock, + pthread_mutex_t &arg_mutex, unsigned quota_per_queue) : + common_driver_mutex_(arg_mutex), + prioritized_tx_queue_(allocator, clock, quota_per_queue), + rx_queue_(allocator, quota_per_queue) + { + } + + ~VirtualCanIface() + { + } + + /** + * Note that RX queue overwrites oldest items when overflowed. + * Call this from the main thread only. + * No additional locking is required. + */ + void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) + { + Lock lock(common_driver_mutex_); + + if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) { + rx_queue_.pop(); + (void)rx_queue_.tryEmplace(frame, flags); + } + } + + /** + * Call this from the main thread only. + * No additional locking is required. + */ + void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index) + { + Lock lock(common_driver_mutex_); + const std::uint8_t iface_mask = static_cast(1U << iface_index); + + while (auto e = prioritized_tx_queue_.peek()) { + UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s", + unsigned(iface_mask), e->toString().c_str()); + + const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask, + uavcan::CanTxQueue::Qos(e->qos), e->flags); + + prioritized_tx_queue_.remove(e); + + if (res <= 0) { + break; + } + + } + } + + /** + * Call this from the sub-node thread only. + * No additional locking is required. + */ + bool hasDataInRxQueue() + { + Lock lock(common_driver_mutex_); + return !rx_queue_.isEmpty(); + } +}; + +/** + * This interface defines one method that will be called by the main node thread periodically in order to + * transfer contents of TX queue of the sub-node into the TX queue of the main node. + */ +class ITxQueueInjector +{ +public: + virtual ~ITxQueueInjector() { } + + /** + * Flush contents of TX queues into the main node. + * @param main_node Reference to the main node. + */ + virtual void injectTxFramesInto(uavcan::INode &main_node) = 0; +}; + +/** + * Objects of this class are owned by the sub-node thread. + * This class does not use heap memory. + * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the + * memory pool that will be shared across all interfaces for RX/TX queues. + * Typically this value should be no less than 4K per interface. + */ +template +class VirtualCanDriver : public uavcan::ICanDriver, + public uavcan::IRxFrameListener, + public ITxQueueInjector, + uavcan::Noncopyable +{ + class Event + { + FAR sem_t sem; + + + public: + + int init() + { + return sem_init(&sem, 0, 0); + } + + int deinit() + { + return sem_destroy(&sem); + } + + + Event() + { + } + + ~Event() + { + } + + + /** + */ + + void waitFor(uavcan::MonotonicDuration duration) + { + static const unsigned NsPerSec = 1000000000; + + if (duration.isPositive()) { + auto abstime = ::timespec(); + + if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) { + abstime.tv_nsec += duration.toUSec() * 1000; + + if (abstime.tv_nsec >= NsPerSec) { + abstime.tv_sec++; + abstime.tv_nsec -= NsPerSec; + } + + (void)sem_timedwait(&sem, &abstime); + } + } + } + + void signal() + { + int count; + int rv = sem_getvalue(&sem, &count); + + if (rv > 0 && count <= 0) { + sem_post(&sem); + } + } + }; + + Event event_; ///< Used to unblock the select() call when IO happens. + pthread_mutex_t driver_mutex_; ///< Shared across all ifaces + uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; + const unsigned num_ifaces_; + uavcan::ISystemClock &clock_; + + uavcan::ICanIface *getIface(uint8_t iface_index) override + { + return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr; + } + + uint8_t getNumIfaces() const override { return num_ifaces_; } + + /** + * This and other methods of ICanDriver will be invoked by the sub-node thread. + */ + int16_t select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline) override + { + bool need_block = (inout_masks.write == 0); // Write queue is infinite + + for (unsigned i = 0; need_block && (i < num_ifaces_); i++) { + const bool need_read = inout_masks.read & (1U << i); + + if (need_read && ifaces_[i]->hasDataInRxQueue()) { + need_block = false; + } + } + + if (need_block) { + event_.waitFor(blocking_deadline - clock_.getMonotonic()); + } + + inout_masks = uavcan::CanSelectMasks(); + + for (unsigned i = 0; i < num_ifaces_; i++) { + const std::uint8_t iface_mask = 1U << i; + inout_masks.write |= iface_mask; // Always ready to write + + if (ifaces_[i]->hasDataInRxQueue()) { + inout_masks.read |= iface_mask; + } + } + + return num_ifaces_; // We're always ready to write, hence > 0. + } + + /** + * This handler will be invoked by the main node thread. + */ + void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override + { + UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str()); + + if (frame.iface_index < num_ifaces_) { + ifaces_[frame.iface_index]->addRxFrame(frame, flags); + event_.signal(); + + } + } + + /** + * This method will be invoked by the main node thread. + */ + void injectTxFramesInto(uavcan::INode &main_node) override + { + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i]->flushTxQueueTo(main_node, i); + } + + event_.signal(); + } + +public: + VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + num_ifaces_(arg_num_ifaces), + clock_(system_clock) + { + Lock::init(driver_mutex_); + event_.init(); + + assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); + + const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; + const unsigned quota_per_queue = quota_per_iface; // 2x overcommit + + UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", + unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + + for (unsigned i = 0; i < num_ifaces_; i++) { + ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + } + } + + ~VirtualCanDriver() + { + Lock::deinit(driver_mutex_); + event_.deinit(); + } + +}; From 67f1fbf84418e96a7b21b3825a34d125d6c309ed Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sun, 21 Jun 2015 09:15:59 -1000 Subject: [PATCH 002/136] Need recusive submodule checkout --- Tools/check_submodules.sh | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4b251642ce..c2c6e3d7bb 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ]; exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi @@ -70,8 +68,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d src/lib/eigen ] @@ -92,8 +89,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/gencpp ] @@ -114,8 +110,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi if [ -d Tools/genmsg ] @@ -136,8 +131,7 @@ then exit 1 fi else - git submodule init; - git submodule update; + git submodule update --init --recursive fi exit 0 From fc6144c46ed952816226ae4d8620efa221b696c6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 22 Jul 2015 09:17:32 -1000 Subject: [PATCH 003/136] Updated Submodule src/lib/uavcan ==master --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 4d18f381e5..4238df3e9f 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 4d18f381e580dc0e086a8fb69469fa28e88244bc +Subproject commit 4238df3e9fe4a01b22d55fb7088319017bdbab8b From 005d0cb0e7d97c3adb5a4f28064b40bded0e61cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Aug 2015 10:04:36 +0200 Subject: [PATCH 004/136] Att pos EKF: Fix init logic, only set local reference if GPS lock present --- .../AttitudePositionEstimatorEKF.h | 2 +- .../ekf_att_pos_estimator_main.cpp | 27 ++++++++++--------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 0dd2b72d92..b1c13ee553 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -335,7 +335,7 @@ private: /** * Initialize the reference position for the local coordinate frame */ - void initReferencePosition(hrt_abstime timestamp, + void initReferencePosition(hrt_abstime timestamp, bool gps_valid, double lat, double lon, float gps_alt, float baro_alt); /** 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 60be85b2cd..07b8c8dfd5 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 @@ -420,7 +420,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, lat, lon, gps_alt, _baro.altitude); } else if (_ekf_logging) { _ekf->GetFilterState(&ekf_report); @@ -726,7 +726,7 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (isfinite(_ekf->states[9])) { @@ -738,17 +738,20 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, } // 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; + if (gps_valid) { + _gps_alt_filt = gps_alt; - 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); + // 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_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + } } void AttitudePositionEstimatorEKF::initializeGPS() @@ -781,7 +784,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); - initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); + initReferencePosition(_gps.timestamp_position, _gpsIsGood, 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, @@ -1358,7 +1361,7 @@ void AttitudePositionEstimatorEKF::pollData() } //Check if the GPS fix is good enough for us to use - if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + if ((_gps.fix_type >= 3) && (_gps.eph < requiredAccuracy) && (_gps.epv < requiredAccuracy)) { _gpsIsGood = true; } else { From 4c79f8d4a2aac61db8f4c80ae3459cc87c103914 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 8 Aug 2015 16:15:11 +0200 Subject: [PATCH 005/136] Backported VTOL to beta branch. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 14 +++-- ROMFS/px4fmu_common/init.d/rcS | 4 ++ ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 6 +- .../fw_att_control/fw_att_control_main.cpp | 9 +-- src/modules/vtol_att_control/module.mk | 4 +- src/modules/vtol_att_control/tiltrotor.cpp | 58 ++++++++----------- src/modules/vtol_att_control/tiltrotor.h | 3 +- .../vtol_att_control/tiltrotor_params.c | 39 +------------ .../vtol_att_control_main.cpp | 6 ++ .../vtol_att_control/vtol_att_control_main.h | 2 + .../vtol_att_control_params.c | 48 ++++++++++++++- src/modules/vtol_att_control/vtol_type.h | 4 +- 12 files changed, 109 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 1ec65dff3e..52c6258c39 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_P 0.17 param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_FF 0.0 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.8 param set MC_YAW_FF 0.5 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + + param set VT_TILT_MC 0.08 + param set VT_TILT_TRANS 0.5 + param set VT_TILT_FW 0.9 fi set MIXER firefly6 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index dfa270b048..d45cb7c97a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -626,6 +626,10 @@ then then set MAV_TYPE 21 fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi fi # Still no MAV_TYPE found diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 22dc2a69ce..8e31007ff1 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -5,18 +5,18 @@ Tilt mechanism servo mixer --------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 10000 10000 0 -10000 10000 +S: 1 4 0 20000 -10000 -10000 10000 Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer 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 43c44aaeac..9a92f5c70e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -799,6 +798,11 @@ FixedwingAttitudeControl::task_main() //warnx("_actuators_airframe.control[1] = -1.0f;"); } + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { + continue; + } + /* default flaps to center */ float flaps_control = 0.0f; @@ -808,11 +812,8 @@ FixedwingAttitudeControl::task_main() } /* decide if in stabilized or full manual control */ - if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale around tuning airspeed */ - float airspeed; /* if airspeed is not updating, we assume the normal average speed */ diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index ad6efd2b27..d3f9326b04 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \ tiltrotor_params.c \ tiltrotor.cpp \ vtol_type.cpp \ - tailsitter.cpp + tailsitter.cpp \ + standard_params.c \ + standard.cpp EXTRACXXFLAGS = -Wno-write-strings diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2cf074fe42..e7ebab58e9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state() void Tiltrotor::process_mc_data() { - fill_mc_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_fw_state() @@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data() void Tiltrotor::process_fw_data() { - fill_fw_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_transition_state() { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition) { - _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + if (_params_tiltrotor.front_trans_dur <= 0.0f) { + _tilt_control = _params_tiltrotor.tilt_transition; + } else if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) { _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); } else { // at low speeds give full weight to mc @@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state() _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f); _roll_weight_mc = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { // tilt rotors forward up to certain angle - float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress; } _roll_weight_mc = progress; @@ -263,38 +267,26 @@ void Tiltrotor::update_external_state() } /** -* Prepare message to acutators with data from mc attitude controller. +* Prepare message to acutators with data from the attitude controllers. */ -void Tiltrotor::fill_mc_att_control_output() +void Tiltrotor::fill_att_control_output() { - _actuators_out_0->control[0] = _actuators_mc_in->control[0]; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2]; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle + } else { + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle + } _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon - + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control -} -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void Tiltrotor::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - /*controls for the elevons */ - _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon - _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle - _actuators_out_1->control[4] = _tilt_control; + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw } /** diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 3ef1362d00..07f3562027 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -99,8 +99,7 @@ private: float _tilt_control; float _roll_weight_mc; - void fill_mc_att_control_output(); - void fill_fw_att_control_output(); + void fill_att_control_output(); void set_max_mc(); void set_max_fw(unsigned pwm_value); diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 7d233f6f50..d4e925f072 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -39,28 +39,6 @@ */ #include - -/** - * Duration of a front transition - * - * Time in seconds used for a transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); - -/** - * Duration of a back transition - * - * Time in seconds used for a back transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); /** * Position of tilt servo in mc mode @@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); /** * Position of tilt servo in transition mode @@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); /** * Position of tilt servo in fw mode @@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); - -/** - * Transition airspeed - * - * Airspeed at which we can switch to fw mode - * - * @min 0.0 - * @max 20 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index b70cd19dd8..0d462423b6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() : } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { + _standard = new Standard(this); + _vtol_type = _standard; } else { _task_should_exit = true; } @@ -530,6 +533,9 @@ void VtolAttitudeControl::task_main() // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); + + _vtol_type->process_mc_data(); + fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 43e8969929..bf9a69d5c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include "tiltrotor.h" #include "tailsitter.h" +#include "standard.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); @@ -188,6 +189,7 @@ private: VtolType * _vtol_type; // base class for different vtol types Tiltrotor * _tiltrotor; // tailsitter vtol type Tailsitter * _tailsitter; // tiltrotor vtol type + Standard * _standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 33c095036c..56779ca8f9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** - * VTOL Type (Tailsitter=0, Tiltrotor=1) + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * * @min 0 - * @max 1 + * @max 2 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); @@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 1.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index bbe6a8642e..a7fdbdde1a 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -38,8 +38,8 @@ * */ -#ifndef VTOL_YYPE_H -#define VTOL_YYPE_H +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H struct Params { int idle_pwm_mc; // pwm value for idle in mc mode From 9ef34b5d1cb2dbf6f5f61ed98031020274b3af42 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 8 Aug 2015 16:16:59 +0200 Subject: [PATCH 006/136] Add standard VTOL airframe. --- .../init.d/13005_vtol_AAERT_quad | 50 +++ .../px4fmu_common/init.d/13005_vtol_AERT_quad | 50 +++ ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix | 32 ++ ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix | 30 ++ .../px4fmu_common/mixers/vtol_quad_x.main.mix | 4 + src/modules/vtol_att_control/standard.cpp | 296 ++++++++++++++++++ src/modules/vtol_att_control/standard.h | 107 +++++++ .../vtol_att_control/standard_params.c | 51 +++ 8 files changed, 620 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad create mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix create mode 100644 ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix create mode 100644 src/modules/vtol_att_control/standard.cpp create mode 100644 src/modules/vtol_att_control/standard.h create mode 100644 src/modules/vtol_att_control/standard_params.c diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad new file mode 100644 index 0000000000..4b8191f557 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad @@ -0,0 +1,50 @@ +#!nsh +# +# @name Generic AAERT tailplane airframe with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Simon Wilks +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AAERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad new file mode 100644 index 0000000000..a3e183a84a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad @@ -0,0 +1,50 @@ +#!nsh +# +# @name Generic AERT with Quad VTOL. +# +# @type Standard VTOL +# +# @maintainer Simon Wilks +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF == yes ] +then + param set VT_TYPE 2 + param set VT_MOT_COUNT 4 + param set VT_TRANS_THR 0.75 + + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + +set MIXER vtol_quad_x +set PWM_OUT 12345678 + +set MIXER_AUX vtol_AERT +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 + +set MAV_TYPE 22 + +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix new file mode 100644 index 0000000000..f3e9c42354 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix @@ -0,0 +1,32 @@ +Mixer for an AAERT VTOL +======================= + +Aileron 1 mixer +--------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Aileron 2 mixer +--------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Elevator mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 + +Throttle mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix new file mode 100644 index 0000000000..260e60840a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix @@ -0,0 +1,30 @@ +Mixer for a horizontal format with X-Quad and tractor AERT plane configuration +============================================================================== + +Motor speed mixer +----------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 3 0 20000 -10000 -10000 10000 + +Aileron mixers +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 + +Elevator mixer +-------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix new file mode 100644 index 0000000000..92bfce6c8c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_quad_x.main.mix @@ -0,0 +1,4 @@ +# VTOL quad X mixer for PX4FMU +#============================= + +R: 4x 10000 10000 10000 0 diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp new file mode 100644 index 0000000000..c38a935ff7 --- /dev/null +++ b/src/modules/vtol_att_control/standard.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file standard.cpp + * + * @author Simon Wilks + * @author Roman Bapst + * +*/ + +#include "standard.h" +#include "vtol_att_control_main.h" + +Standard::Standard(VtolAttitudeControl *attc) : + VtolType(attc), + _flag_enable_mc_motors(true), + _pusher_throttle(0.0f), + _mc_att_ctl_weight(1.0f), + _airspeed_trans_blend_margin(0.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); + _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); + } + +Standard::~Standard() +{ +} + +int +Standard::parameters_update() +{ + float v; + + /* duration of a forwards transition to fw mode */ + param_get(_params_handles_standard.front_trans_dur, &v); + _params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* duration of a back transition to mc mode */ + param_get(_params_handles_standard.back_trans_dur, &v); + _params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + + /* target throttle value for pusher motor during the transition to fw mode */ + param_get(_params_handles_standard.pusher_trans, &v); + _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); + + /* airspeed at which it we should switch to fw mode */ + param_get(_params_handles_standard.airspeed_trans, &v); + _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); + + /* airspeed at which we start blending mc/fw controls */ + param_get(_params_handles_standard.airspeed_blend, &v); + _params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f); + + _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; + + return OK; +} + +void Standard::update_vtol_state() +{ + parameters_update(); + + /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * For the back transition the pusher motor is immediately stopped and rotors reactivated. + */ + + if (_manual_control_sp->aux1 < 0.0f) { + // the transition to fw mode switch is off + if (_vtol_schedule.flight_mode == MC_MODE) { + // in mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // transition to mc mode + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // failsafe back to mc mode + _vtol_schedule.flight_mode = MC_MODE; + _mc_att_ctl_weight = 1.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // keep transitioning to mc mode + _vtol_schedule.flight_mode = MC_MODE; + } + + // the pusher motor should never be powered when in or transitioning to mc mode + _pusher_throttle = 0.0f; + + } else { + // the transition to fw mode switch is on + if (_vtol_schedule.flight_mode == MC_MODE) { + // start transition to fw mode + _vtol_schedule.flight_mode = TRANSITION_TO_FW; + _vtol_schedule.transition_start = hrt_absolute_time(); + + } else if (_vtol_schedule.flight_mode == FW_MODE) { + // in fw mode + _vtol_schedule.flight_mode = FW_MODE; + _mc_att_ctl_weight = 0.0f; + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + _vtol_schedule.flight_mode = FW_MODE; + // we can turn off the multirotor motors now + _flag_enable_mc_motors = false; + // don't set pusher throttle here as it's being ramped up elsewhere + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // transitioning to mc mode & transition switch on - failsafe back into fw mode + _vtol_schedule.flight_mode = FW_MODE; + } + } + + // map specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; + } +} + +void Standard::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_params_standard.front_trans_dur <= 0.0f) { + // just set the final target throttle value + _pusher_throttle = _params_standard.pusher_trans; + } else if (_pusher_throttle <= _params_standard.pusher_trans) { + // ramp up throttle to the target throttle value + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + } + + // do blending of mc and fw controls if a blending airspeed has been provided + if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { + _mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + } else { + // at low speeds give full weight to mc + _mc_att_ctl_weight = 1.0f; + } + + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + // continually increase mc attitude control as we transition back to mc mode + if (_params_standard.back_trans_dur > 0.0f) { + _mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + } else { + _mc_att_ctl_weight = 1.0f; + } + + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (_flag_enable_mc_motors) { + set_max_mc(2000); + set_idle_mc(); + _flag_enable_mc_motors = false; + } + } + + _mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f); +} + +void Standard::update_mc_state() +{ + // do nothing +} + +void Standard::process_mc_data() +{ + fill_att_control_output(); +} + +void Standard::process_fw_data() +{ + fill_att_control_output(); +} + + void Standard::update_fw_state() +{ + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + if (!_flag_enable_mc_motors) { + set_max_mc(950); + set_idle_fw(); // force them to stop, not just idle + _flag_enable_mc_motors = true; + } + } + +void Standard::update_external_state() +{ +} + +/** + * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine + * what proportion of control should be applied to each of the control groups (mc and fw). + */ +void Standard::fill_att_control_output() +{ + /* multirotor controls */ + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle + + /* fixed wing controls */ + const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight; + _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch + _actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw + + // set the fixed wing throttle control + if (_vtol_schedule.flight_mode == FW_MODE) { + // take the throttle value commanded by the fw controller + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; + } else { + // otherwise we may be ramping up the throttle during the transition to fw mode + _actuators_out_1->control[3] = _pusher_throttle; + } +} + +/** +* Disable all multirotor motors when in fw mode. +*/ +void +Standard::set_max_mc(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h new file mode 100644 index 0000000000..e4e1d92d85 --- /dev/null +++ b/src/modules/vtol_att_control/standard.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file standard.h + * VTOL with fixed multirotor motor configurations (such as quad) and a pusher + * (or puller aka tractor) motor for forward flight. + * + * @author Simon Wilks + * @author Roman Bapst + * + */ + +#ifndef STANDARD_H +#define STANDARD_H +#include "vtol_type.h" +#include +#include + +class Standard : public VtolType +{ + +public: + + Standard(VtolAttitudeControl * _att_controller); + ~Standard(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float pusher_trans; + float airspeed_blend; + float airspeed_trans; + } _params_standard; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t pusher_trans; + param_t airspeed_blend; + param_t airspeed_trans; + } _params_handles_standard; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_TO_FW, + TRANSITION_TO_MC, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + }_vtol_schedule; + + bool _flag_enable_mc_motors; + float _pusher_throttle; + float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning + float _airspeed_trans_blend_margin; + + void fill_att_control_output(); + void set_max_mc(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c new file mode 100644 index 0000000000..134dcd0b8a --- /dev/null +++ b/src/modules/vtol_att_control/standard_params.c @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file standard_params.c + * Parameters for the standard VTOL controller. + * + * @author Simon Wilks + * @author Roman Bapst + */ + +#include + +/** + * Target throttle value for pusher/puller motor during the transition to fw mode + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f); From 6b6fe5d22977bb035833b88d6c962d7b1b20e0a9 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 9 Aug 2015 11:36:13 +0200 Subject: [PATCH 007/136] Remove old config and mixer file that slipped into the port. --- .../px4fmu_common/init.d/13005_vtol_AERT_quad | 50 ------------------- ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix | 30 ----------- 2 files changed, 80 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad delete mode 100644 ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad b/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad deleted file mode 100644 index a3e183a84a..0000000000 --- a/ROMFS/px4fmu_common/init.d/13005_vtol_AERT_quad +++ /dev/null @@ -1,50 +0,0 @@ -#!nsh -# -# @name Generic AERT with Quad VTOL. -# -# @type Standard VTOL -# -# @maintainer Simon Wilks -# - -sh /etc/init.d/rc.vtol_defaults - -if [ $AUTOCNF == yes ] -then - param set VT_TYPE 2 - param set VT_MOT_COUNT 4 - param set VT_TRANS_THR 0.75 - - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 - param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 - param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 - param set MC_YAW_FF 0.5 - param set MC_YAWRATE_P 0.22 - param set MC_YAWRATE_I 0.02 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_FF 0.0 -fi - -set MIXER vtol_quad_x -set PWM_OUT 12345678 - -set MIXER_AUX vtol_AERT -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 - -set MAV_TYPE 22 - -param set VT_MOT_COUNT 4 -param set VT_IDLE_PWM_MC 1080 -param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix deleted file mode 100644 index 260e60840a..0000000000 --- a/ROMFS/px4fmu_common/mixers/vtol_AERT.aux.mix +++ /dev/null @@ -1,30 +0,0 @@ -Mixer for a horizontal format with X-Quad and tractor AERT plane configuration -============================================================================== - -Motor speed mixer ------------------ -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 3 0 20000 -10000 -10000 10000 - -Aileron mixers --------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 - -Elevator mixer --------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 1 10000 10000 0 -10000 10000 - -Rudder mixer ------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 1 2 -10000 -10000 0 -10000 10000 From db975a10c2c41330eaa9cf2ba49d1e2625fd46e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Aug 2015 12:29:42 +0200 Subject: [PATCH 008/136] Fix coning handling in att_pos EKF. Fixes #2663 --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 8 ++++---- src/modules/ekf_att_pos_estimator/estimator_22states.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 967402fff7..3177ab75ef 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -96,6 +96,7 @@ AttPosEKF::AttPosEKF() : correctedDelVel(), summedDelAng(), summedDelVel(), + prevDelAng(), accNavMag(), earthRateNED(), angRate(), @@ -273,12 +274,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() delAngTotal.y += correctedDelAng.y; delAngTotal.z += correctedDelAng.z; - // Save current measurements - Vector3f prevDelAng = correctedDelAng; - // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + prevDelAng = correctedDelAng; // Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); @@ -3290,6 +3289,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + prevDelAng.zero(); dAngIMU.zero(); dVelIMU.zero(); lastGyroOffset.zero(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 9b23f4df44..c9a217a2bb 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -139,6 +139,7 @@ public: Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f prevDelAng; ///< previous delta angle, used for coning correction float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) From c287b03d1dd50e5ad84e28b5b87ee65bfc40bece Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Aug 2015 23:27:16 +0200 Subject: [PATCH 009/136] Commander: Do not spam console when not connected --- src/modules/commander/commander.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a01b33087f..b36cb7480b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1361,7 +1361,9 @@ int commander_thread_main(int argc, char *argv[]) /* we first connect a link or re-connect a link after loosing it */ (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) && /* and this link has a communication partner */ - telemetry.heartbeat_time > 0 && + (telemetry.heartbeat_time > 0) && + /* and it is still connected */ + (hrt_elapsed_time(&telemetry.heartbeat_time) < 2 * 1000 * 1000) && /* and the system is not already armed (and potentially flying) */ !armed.armed) { From c5ec4de6eab8aa7639f263aa42c88f0d76061820 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Aug 2015 23:26:35 +0200 Subject: [PATCH 010/136] Increase NSH back-off time --- src/systemcmds/nshterm/nshterm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bdfd26a64d..320d338a23 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -68,8 +68,8 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; - /* back off 1500 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 1500U * 1000U) { + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { usleep(50000); } From 9fa87d1444c5fb8f258914eb917aff660dfcffc2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Aug 2015 04:45:32 +0300 Subject: [PATCH 011/136] LIbuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 4238df3e9f..c2fec7be38 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 4238df3e9fe4a01b22d55fb7088319017bdbab8b +Subproject commit c2fec7be38bcf8fdfb1369f28006c77f7d509d82 From e278419e849d56e24e79e7601072879a0be594d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Aug 2015 05:12:46 +0300 Subject: [PATCH 012/136] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c2fec7be38..46793c5e06 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c2fec7be38bcf8fdfb1369f28006c77f7d509d82 +Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 From aa412aacede3f8ced508600fcf94d8d8247b2fce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Aug 2015 09:42:16 +0300 Subject: [PATCH 013/136] UAVCAN servers: proper handling of startup failure, more verbose error reporting --- src/modules/uavcan/uavcan_servers.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 6c4d70e3cd..d6d72a2f53 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -142,8 +142,9 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) int rv = _instance->init(num_ifaces); if (rv < 0) { - warnx("Node init failed %i", rv); + warnx("Node init failed: %d", rv); delete _instance; + _instance = nullptr; return rv; } @@ -163,9 +164,10 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(run_trampoline), NULL); if (rv < 0) { - warnx("start failed: %d", errno); + warnx("pthread_create() failed: %d", errno); rv = -errno; delete _instance; + _instance = nullptr; } return rv; @@ -173,6 +175,8 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) int UavcanServers::init(unsigned num_ifaces) { + errno = 0; + /* * Initialize the mutex. * giving it its path @@ -181,6 +185,7 @@ int UavcanServers::init(unsigned num_ifaces) int ret = Lock::init(_subnode_mutex); if (ret < 0) { + warnx("Lock init: %d", errno); return ret; } @@ -197,6 +202,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); if (ret < 0) { + warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno); return ret; } @@ -205,6 +211,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_server.start(); if (ret < 0) { + warnx("BasicFileServer init: %d, errno: %d", ret, errno); return ret; } @@ -213,6 +220,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _storage_backend.init(UAVCAN_NODE_DB_PATH); if (ret < 0) { + warnx("FileStorageBackend init: %d, errno: %d", ret, errno); return ret; } @@ -221,6 +229,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _tracer.init(UAVCAN_LOG_FILE); if (ret < 0) { + warnx("FileEventTracer init: %d, errno: %d", ret, errno); return ret; } @@ -232,6 +241,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _server_instance.init(hwver.unique_id); if (ret < 0) { + warnx("CentralizedServer init: %d", ret); return ret; } @@ -240,6 +250,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _node_info_retriever.start(); if (ret < 0) { + warnx("NodeInfoRetriever init: %d", ret); return ret; } @@ -248,6 +259,7 @@ int UavcanServers::init(unsigned num_ifaces) ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); if (ret < 0) { + warnx("FirmwareUpdateTrigger init: %d", ret); return ret; } From 36f91d30ebf6d617e4d3dc3299855a59598b4170 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Aug 2015 09:53:13 +0300 Subject: [PATCH 014/136] Fixes pure virtual call exception in VirtualCanDriver<>::handleRxFrame() Stack trace: Thread [1] (Suspended: Breakpoint hit.) 12 __cxa_pure_virtual() libxx_cxapurevirtual.cxx:66 0x0808ca42 11 notifyRxFrameListener() uc_dispatcher.cpp:216 0x080787ce 10 uavcan::Dispatcher::spinOnce() uc_dispatcher.cpp:276 0x080787ce 9 uavcan::Scheduler::spinOnce() uc_scheduler.cpp:196 0x0807b4e0 8 spinOnce() abstract_node.hpp:88 0x080659fc 7 spinOnce() node.hpp:132 0x080659fc 6 UavcanNode::node_spin_once() uavcan_main.cpp:428 0x080659fc 5 UavcanNode::run() uavcan_main.cpp:542 0x08065e74 4 operator() uavcan_main.cpp:343 0x0806626a 3 UavcanNode::__lambda0::_FUN() uavcan_main.cpp:343 0x0806626a 2 task_start() task_start.c:138 0x08087720 1 0x00000000 --- src/modules/uavcan/uavcan_servers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index d6d72a2f53..1713b282e7 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -98,6 +98,7 @@ UavcanServers::~UavcanServers() if (_mutex_inited) { (void)Lock::deinit(_subnode_mutex); } + _main_node.getDispatcher().removeRxFrameListener(); } int UavcanServers::stop(void) From 39732abf1f16d4dc9ca643001ef39cf73a462727 Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 11 Aug 2015 11:14:35 -0400 Subject: [PATCH 015/136] Changed ecl_roll/pitch/yaw_controller.cpp to no longer transform ctl_data.angle_rate to angular acceleration as the angular rates are already angular accelerations as calculated by ekf_att_pos_estimator --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 22 ++++++++++--------- .../ecl/attitude_fw/ecl_roll_controller.cpp | 5 +---- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 6 +---- 3 files changed, 14 insertions(+), 19 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index c80eb357cf..943278fbf5 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -96,15 +96,21 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da /* input conditioning */ float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - /* calculate the offset in the rate resulting from rolling */ - //xxx needs explanation and conversion to body angular rates or should be removed - float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll)) * _roll_ff; + + /* Calculate desired y-axis angular rate needed to compensate for roll angle. + For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 + Availible on google books 8/11/2015: + https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ + float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll))); if (inverted) { - turn_offset = -turn_offset; + body_fixed_turn_offset = -body_fixed_turn_offset; } + /*Finally transform the body_fixed_turn offset to a change pitch angular rate */ + float turn_offset = cosf(roll)*body_fixed_turn_offset-sinf(roll)*ctl_data.yaw_rate; + /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; @@ -160,11 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; - - _rate_error = _bodyrate_setpoint - pitch_bodyrate; + _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1c43e7ebf7..5935f15ceb 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* Transform setpoint to body angular rates (jacobian) */ _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates (jacobian) */ - float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4dd409283a..d1b6f1bfeb 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl _bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); } - /* Transform estimation to body angular rates (jacobian) */ - float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate; - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { From bf9bbfa27fc5f2b5ea909600d7dc983998f2ec4d Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 18 Aug 2015 13:32:50 -0400 Subject: [PATCH 016/136] Handle turn pitch rate compensation in pitch rate controller instead of pitch controller. Also set inverted flag to true if rolled more than 90 degrees --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 89 +++++++++---------- 1 file changed, 43 insertions(+), 46 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 943278fbf5..b1304b34bf 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -60,11 +60,10 @@ ECL_PitchController::~ECL_PitchController() float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { - float roll = ctl_data.roll; /* Do not calculate control signal with bad inputs */ if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(roll) && + isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); @@ -72,56 +71,13 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da return _rate_setpoint; } - /* flying inverted (wings upside down) ? */ - bool inverted = false; - - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (roll > 0.0f) { - /* right hemisphere */ - roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - - /* Calculate desired y-axis angular rate needed to compensate for roll angle. - For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 - Availible on google books 8/11/2015: - https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ - float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(roll) * sinf(roll))); - - if (inverted) { - body_fixed_turn_offset = -body_fixed_turn_offset; - } - - /*Finally transform the body_fixed_turn offset to a change pitch angular rate */ - float turn_offset = cosf(roll)*body_fixed_turn_offset-sinf(roll)*ctl_data.yaw_rate; - /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; - /* add turn offset */ - _rate_setpoint += turn_offset; - - /* limit the rate */ //XXX: move to body angluar rates - + /* limit the rate */ if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_rate_setpoint > 0.0f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; @@ -166,6 +122,47 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; + /* apply turning offset to desired bodyrate setpoint*/ + /* flying inverted (wings upside down)*/ + bool inverted = false; + float constrained_roll; + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(ctl_data.roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); + + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + inverted = true; + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (ctl_data.roll > 0.0f) { + /* right hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); + + } else { + /* left hemisphere */ + constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + + /* input conditioning */ + float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); + + /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. + For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 + Availible on google books 8/11/2015: + https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ + float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(constrained_roll) * sinf(constrained_roll))); + + if (inverted) { + body_fixed_turn_offset = -body_fixed_turn_offset; + } + + /* Finally add the turn offset to your bodyrate setpoint*/ + _bodyrate_setpoint += body_fixed_turn_offset; + + _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; if (!lock_integrator && _k_i > 0.0f) { From 62269004c88a085bf106672f6ce41b3690233499 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 10:43:35 +0200 Subject: [PATCH 017/136] FW control: Remove unused parameters --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 ------- src/modules/fw_att_control/fw_att_control_params.c | 11 ----------- 2 files changed, 18 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 c3f83e21ba..d073289952 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -168,7 +168,6 @@ private: float p_rmax_pos; float p_rmax_neg; float p_integrator_max; - float p_roll_feedforward; float r_p; float r_i; float r_ff; @@ -178,7 +177,6 @@ private: float y_i; float y_d; float y_ff; - float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; int32_t y_coordinated_method; @@ -211,7 +209,6 @@ private: param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; - param_t p_roll_feedforward; param_t r_p; param_t r_i; param_t r_ff; @@ -221,7 +218,6 @@ private: param_t y_i; param_t y_d; param_t y_ff; - param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; param_t y_coordinated_method; @@ -374,7 +370,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); - _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_i = param_find("FW_RR_I"); @@ -450,7 +445,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); - param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); @@ -493,7 +487,6 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index ed026c02eb..5e640fcc12 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -128,17 +128,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); */ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); -/** - * Roll to Pitch feedforward gain. - * - * This compensates during turns and ensures the nose stays level. - * - * @min 0.0 - * @max 2.0 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); - /** * Roll rate proportional Gain * From aaafbad9e7f37837cd6f4adc801ab7d46ffc7d7f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 5 Aug 2015 10:24:34 -1000 Subject: [PATCH 018/136] Updated Config - Using Irq stack and reduces idel stack and user sizes --- nuttx-configs/px4io-v2/nsh/defconfig | 1182 +++++++++++++++----------- 1 file changed, 680 insertions(+), 502 deletions(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index a9b8477948..801e451018 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -1,198 +1,705 @@ -############################################################################ -# configs/px4io/nsh/defconfig # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration # -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: +CONFIG_NUTTX_NEWCONFIG=y + # -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name NuttX nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. +# Build Setup # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + # -############################################################################ +# Build Configuration # -# architecture selection +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + # -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization +# Binary Output Formats # -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io-v2" -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +# CONFIG_ARMV7M_USEBASEPRI is not set +CONFIG_ARCH_HAVE_CMNVECTOR=y CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_ARMV7M_STACKCHECK=n -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_ARCH_HAVE_FPU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set # -# Individual subsystems can be enabled: +# ARMV7M Configuration Options # -# AHB: +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +CONFIG_ARCH_CHIP_STM32F100C8=y +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +CONFIG_STM32_STM32F10XX=y +CONFIG_STM32_VALUELINE=y +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +# CONFIG_STM32_STM32F40XX is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_BKP is not set +# CONFIG_STM32_CEC is not set +# CONFIG_STM32_CRC is not set CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_I2C1 is not set +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_TIM15 is not set +# CONFIG_STM32_TIM16 is not set +# CONFIG_STM32_TIM17 is not set +CONFIG_STM32_USART1=y CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=n -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y # -# STM32F100 specific serial device driver settings +# Alternate Pin Mapping # -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits +# CONFIG_STM32_USART1_REMAP is not set +# CONFIG_STM32_USART2_REMAP is not set +CONFIG_STM32_USART3_NO_REMAP=y +# CONFIG_STM32_USART3_FULL_REMAP is not set +# CONFIG_STM32_USART3_PARTIAL_REMAP is not set +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG is not set +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_STM32_USART=y + # -CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y - -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +# CONFIG_USART2_RS485 is not set +# CONFIG_USART2_RXDMA is not set +# CONFIG_USART3_RS485 is not set CONFIG_USART3_RXDMA=y +# CONFIG_SERIAL_DISABLE_REORDERING is not set +# CONFIG_STM32_USART_SINGLEWIRE is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=2000 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=0x2000 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=400 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y + +# +# Common Board Options +# + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_TASK_NAME_SIZE=8 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +# CONFIG_SCHED_WAITPID is not set +# CONFIG_SCHED_STARTHOOK is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="user_start" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_ENVIRON=y + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=290 +CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 + +# +# Device Drivers +# +CONFIG_DISABLE_POLL=y +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +# CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_SPI is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART1_SERIAL_CONSOLE=y +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +CONFIG_DISABLE_MOUNTPOINT=y +# CONFIG_FS_RAMMAP is not set + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +CONFIG_MM_SMALL=y +CONFIG_MM_REGIONS=1 +# CONFIG_GRAN is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +# CONFIG_STDIO_LINEBUFFER is not set +CONFIG_NUNGET_CHARS=2 +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Non-standard Library Support +# +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set + +# +# USB Monitor +# # # PX4IO specific driver settings @@ -216,332 +723,3 @@ CONFIG_USART3_RXDMA=y CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=8 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# - -# Disable NSH completely -CONFIG_NSH_CONSOLE=n - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1200 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= From abc74323ac9ecf9534d7ac7139f8aef3911a3edb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 5 Aug 2015 12:09:36 -1000 Subject: [PATCH 019/136] Set margins CONFIG_ARCH_INTERRUPTSTACK = 500 40 Bytes margin CONFIG_IDLETHREAD_STACKSIZE= 290 - idel usage 0x20001F78 from 0x20002000 is 136 bytes Margin is 154 CONFIG_USERMAIN_STACKSIZE=800 268 bytes margin 0x200013c8 --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 801e451018..56892e3f93 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -276,7 +276,7 @@ CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=0x2000 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=400 +CONFIG_ARCH_INTERRUPTSTACK=500 # # Boot options From 8ec5b0321150855d97ad5c6284865d661c7efa9a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 16:29:54 +0200 Subject: [PATCH 020/136] Disable VTOL airframes since they are not fully stable in beta --- .../{13001_caipirinha_vtol => INACTIVE_13001_caipirinha_vtol} | 0 .../init.d/{13002_firefly6 => INACTIVE_13002_firefly6} | 0 .../{13003_quad_tailsitter => INACTIVE_13003_quad_tailsitter} | 0 .../{13004_quad+_tailsitter => INACTIVE_13004_quad+_tailsitter} | 0 .../{13005_vtol_AAERT_quad => INACTIVE_13005_vtol_AAERT_quad} | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/init.d/{13001_caipirinha_vtol => INACTIVE_13001_caipirinha_vtol} (100%) rename ROMFS/px4fmu_common/init.d/{13002_firefly6 => INACTIVE_13002_firefly6} (100%) rename ROMFS/px4fmu_common/init.d/{13003_quad_tailsitter => INACTIVE_13003_quad_tailsitter} (100%) rename ROMFS/px4fmu_common/init.d/{13004_quad+_tailsitter => INACTIVE_13004_quad+_tailsitter} (100%) rename ROMFS/px4fmu_common/init.d/{13005_vtol_AAERT_quad => INACTIVE_13005_vtol_AAERT_quad} (100%) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol similarity index 100% rename from ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol rename to ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 similarity index 100% rename from ROMFS/px4fmu_common/init.d/13002_firefly6 rename to ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/13003_quad_tailsitter rename to ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter rename to ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad similarity index 100% rename from ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad rename to ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad From e8f4aa4de8faa2a10a1e08f84ed0bc0a2054d4d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 20:33:36 +0200 Subject: [PATCH 021/136] Include eigen dir as include dir --- makefiles/firmware.mk | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 4b436072ba..c59b66b21f 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -181,6 +181,12 @@ EXTRA_CLEANS = # INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) +################################################################################ +# External library includes +################################################################################ + +INCLUDE_DIRS += $(PX4_BASE)src/lib/eigen/ + ################################################################################ # OS specific libraries and paths ################################################################################ From 8201c6ee1b93657e2d7935693e8ff7f177e5d529 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 22:35:45 +0200 Subject: [PATCH 022/136] Update Eigen --- src/lib/eigen | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/eigen b/src/lib/eigen index e7850ed81f..240599e148 160000 --- a/src/lib/eigen +++ b/src/lib/eigen @@ -1 +1 @@ -Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 +Subproject commit 240599e1488da540c2ae8753dea0907f6cfac8ee From a4e06654ff3d80d016c6352870bc105ddc8fa4f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 22:39:50 +0200 Subject: [PATCH 023/136] Eigen: Remove unneeded config option --- src/lib/px4_eigen.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h index 6c4556c5f5..2cd98e59aa 100644 --- a/src/lib/px4_eigen.h +++ b/src/lib/px4_eigen.h @@ -46,8 +46,6 @@ #pragma GCC diagnostic ignored "-Wfloat-equal" #define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 -#define EIGEN_MAX_STATIC_ALIGN_BYTES 16 - #include #include #pragma GCC diagnostic pop From 33a1e3127a3e92f5fdf9a4aaad19f5ad3e7fa0aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:18:10 +0200 Subject: [PATCH 024/136] Accel: Move to generated uORB topics --- msg/sensor_accel.msg | 13 +++++++++++++ src/drivers/drv_accel.h | 26 ++------------------------ 2 files changed, 15 insertions(+), 24 deletions(-) create mode 100644 msg/sensor_accel.msg diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg new file mode 100644 index 0000000000..fdae381aba --- /dev/null +++ b/msg/sensor_accel.msg @@ -0,0 +1,13 @@ +uint64 timestamp +uint64 error_count +float32 x # acceleration in the NED X board axis in m/s^2 +float32 y # acceleration in the NED Y board axis in m/s^2 +float32 z # acceleration in the NED Z board axis in m/s^2 +float32 temperature # temperature in degrees celsius +float32 range_m_s2 # range in m/s^2 (+- this value) +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 738b28a6e1..f17d1ac9c2 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -51,25 +51,8 @@ #define ACCEL1_DEVICE_PATH "/dev/accel1" #define ACCEL2_DEVICE_PATH "/dev/accel2" -/** - * accel report structure. Reads from the device must be in multiples of this - * structure. - */ -struct accel_report { - uint64_t timestamp; - uint64_t error_count; - float x; /**< acceleration in the NED X board axis in m/s^2 */ - float y; /**< acceleration in the NED Y board axis in m/s^2 */ - float z; /**< acceleration in the NED Z board axis in m/s^2 */ - float temperature; /**< temperature in degrees celsius */ - float range_m_s2; /**< range in m/s^2 (+- this value) */ - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define accel_report sensor_accel_s /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { @@ -81,11 +64,6 @@ struct accel_scale { float z_scale; }; -/* - * ObjDev tag for raw accelerometer data. - */ -ORB_DECLARE(sensor_accel); - /* * ioctl() definitions * From 7e24240ec394ab4e0c011e2891d30b955f9b0f38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:18:35 +0200 Subject: [PATCH 025/136] Baro: Move to generated uORB topics --- msg/sensor_baro.msg | 5 +++++ src/drivers/drv_baro.h | 18 ++---------------- 2 files changed, 7 insertions(+), 16 deletions(-) create mode 100644 msg/sensor_baro.msg diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg new file mode 100644 index 0000000000..315d5a56ec --- /dev/null +++ b/msg/sensor_baro.msg @@ -0,0 +1,5 @@ +float32 pressure +float32 altitude +float32 temperature +uint64 timestamp +uint64 error_count diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 791e3c5cc1..4da8e8050f 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -50,22 +50,8 @@ #define BARO_BASE_DEVICE_PATH "/dev/baro" #define BARO0_DEVICE_PATH "/dev/baro0" -/** - * baro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct baro_report { - float pressure; - float altitude; - float temperature; - uint64_t timestamp; - uint64_t error_count; -}; - -/* - * ObjDev tag for raw barometer data. - */ -ORB_DECLARE(sensor_baro); +#include +#define baro_report sensor_baro_s /* * ioctl() definitions From 1d1431e532a63ba35bb384609ba642826abe1525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:18:57 +0200 Subject: [PATCH 026/136] gyro: Move to generated uORB topics --- msg/sensor_gyro.msg | 13 +++++++++++++ src/drivers/drv_gyro.h | 26 ++------------------------ 2 files changed, 15 insertions(+), 24 deletions(-) create mode 100644 msg/sensor_gyro.msg diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg new file mode 100644 index 0000000000..4a252269e8 --- /dev/null +++ b/msg/sensor_gyro.msg @@ -0,0 +1,13 @@ +uint64 timestamp +uint64 error_count +float32 x # angular velocity in the NED X board axis in rad/s +float32 y # angular velocity in the NED Y board axis in rad/s +float32 z # angular velocity in the NED Z board axis in rad/s +float32 temperature # temperature in degrees celcius +float32 range_rad_s +float32 scaling + +int16 x_raw +int16 y_raw +int16 z_raw +int16 temperature_raw diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 8e80413ac9..feaa38881f 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -51,25 +51,8 @@ #define GYRO1_DEVICE_PATH "/dev/gyro1" #define GYRO2_DEVICE_PATH "/dev/gyro2" -/** - * gyro report structure. Reads from the device must be in multiples of this - * structure. - */ -struct gyro_report { - uint64_t timestamp; - uint64_t error_count; - float x; /**< angular velocity in the NED X board axis in rad/s */ - float y; /**< angular velocity in the NED Y board axis in rad/s */ - float z; /**< angular velocity in the NED Z board axis in rad/s */ - float temperature; /**< temperature in degrees celcius */ - float range_rad_s; - float scaling; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; - int16_t temperature_raw; -}; +#include +#define gyro_report sensor_gyro_s /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ struct gyro_scale { @@ -81,11 +64,6 @@ struct gyro_scale { float z_scale; }; -/* - * ObjDev tag for raw gyro data. - */ -ORB_DECLARE(sensor_gyro); - /* * ioctl() definitions */ From 36a787bd91a50f0b3b16aac93b536f2a636d942d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:19:19 +0200 Subject: [PATCH 027/136] Mag: move to generated uORB topics --- msg/sensor_mag.msg | 12 ++++++++++++ src/drivers/drv_mag.h | 28 ++-------------------------- 2 files changed, 14 insertions(+), 26 deletions(-) create mode 100644 msg/sensor_mag.msg diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg new file mode 100644 index 0000000000..3ac97227ea --- /dev/null +++ b/msg/sensor_mag.msg @@ -0,0 +1,12 @@ +uint64 timestamp +uint64 error_count +float32 x +float32 y +float32 z +float32 range_ga +float32 scaling +float32 temperature + +int16 x_raw +int16 y_raw +int16 z_raw diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e2f8493e38..3abe84c883 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -49,26 +49,8 @@ #define MAG1_DEVICE_PATH "/dev/mag1" #define MAG2_DEVICE_PATH "/dev/mag2" -/** - * mag report structure. Reads from the device must be in multiples of this - * structure. - * - * Output values are in gauss. - */ -struct mag_report { - uint64_t timestamp; - uint64_t error_count; - float x; - float y; - float z; - float range_ga; - float scaling; - float temperature; - - int16_t x_raw; - int16_t y_raw; - int16_t z_raw; -}; +#include +#define mag_report sensor_mag_s /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ struct mag_scale { @@ -80,12 +62,6 @@ struct mag_scale { float z_scale; }; -/* - * ObjDev tag for raw magnetometer data. - */ -ORB_DECLARE(sensor_mag); - - /* * ioctl() definitions */ From b530582e5970f934650c75d22e7e3c9807f9e647 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:19:37 +0200 Subject: [PATCH 028/136] RC param map: Move to generated uORB topics --- msg/rc_parameter_map.msg | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 msg/rc_parameter_map.msg diff --git a/msg/rc_parameter_map.msg b/msg/rc_parameter_map.msg new file mode 100644 index 0000000000..b2dd1843ab --- /dev/null +++ b/msg/rc_parameter_map.msg @@ -0,0 +1,11 @@ +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +uint64 timestamp # time at which the map was updated +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # inital value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value From d7bfdfd2343a60a40c8d53c1b8ffa7831e5e539c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:20:27 +0200 Subject: [PATCH 029/136] MAVLink app: Adjust to changes in uORB topic --- src/modules/mavlink/mavlink_parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 559cf2c89c..414b7ee96f 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -159,9 +159,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; - strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ - _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; From b6fd180d5595283205e160cf644210712443db80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:20:45 +0200 Subject: [PATCH 030/136] sensors app: Adjust to changes in param RC map uORB topic --- src/modules/sensors/sensors.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 274f10c3e9..2626b9a715 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -249,7 +249,7 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; - float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ + float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ @@ -294,7 +294,7 @@ private: int rc_map_aux4; int rc_map_aux5; - int rc_map_param[RC_PARAM_MAP_NCHAN]; + int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; int32_t rc_fails_thr; float rc_assist_th; @@ -350,8 +350,8 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_map_param[RC_PARAM_MAP_NCHAN]; - param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound to a RC channel, equivalent float values in the _parameters struct are not existing because these parameters are never read. */ @@ -603,9 +603,9 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - char name[PARAM_ID_LEN]; - snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -783,7 +783,7 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } @@ -831,7 +831,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } @@ -1678,7 +1678,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink @@ -1691,17 +1691,17 @@ Sensors::rc_parameter_map_poll(bool forced) _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { - _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); } } warnx("rc to parameter map updated"); - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", i, - _rc_parameter_map.param_id[i], + &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)], (double)_rc_parameter_map.scale[i], (double)_rc_parameter_map.value0[i], (double)_rc_parameter_map.value_min[i], @@ -1893,7 +1893,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) void Sensors::set_params_from_rc() { - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink From 647c2e2a3a7d0e0b67f5311d436844b8c80f0618 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:21:28 +0200 Subject: [PATCH 031/136] RC param map: Remove non-generated file from GIT --- src/modules/uORB/topics/rc_parameter_map.h | 76 ---------------------- 1 file changed, 76 deletions(-) delete mode 100644 src/modules/uORB/topics/rc_parameter_map.h diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h deleted file mode 100644 index 6e68dc4b6f..0000000000 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT , - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file rc_parameter_map.h - * Maps RC channels to parameters - * - * @author Thomas Gubler - */ - -#ifndef TOPIC_RC_PARAMETER_MAP_H -#define TOPIC_RC_PARAMETER_MAP_H - -#include -#include "../uORB.h" - -#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h -#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN - -/** - * @addtogroup topics - * @{ - */ - -struct rc_parameter_map_s { - uint64_t timestamp; /**< time at which the map was updated */ - - bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ - - int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this - this field is ignored if set to -1, in this case param_id will - be used*/ - char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ - float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ - float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ - float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ - float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ -}; - -/** - * @} - */ - -ORB_DECLARE(rc_parameter_map); - -#endif From b0a9679fcafd82f147b93eef252a1fd965a7a6d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:46:59 +0200 Subject: [PATCH 032/136] PWM out driver: Move to generated uORB topic --- msg/output_pwm.msg | 3 +++ src/drivers/drv_pwm_output.h | 27 ++++++++------------------- 2 files changed, 11 insertions(+), 19 deletions(-) create mode 100644 msg/output_pwm.msg diff --git a/msg/output_pwm.msg b/msg/output_pwm.msg new file mode 100644 index 0000000000..1418a43e6e --- /dev/null +++ b/msg/output_pwm.msg @@ -0,0 +1,3 @@ +uint8 PWM_OUTPUT_MAX_CHANNELS = 16 +uint16[16] values +uint32 channel_count diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index eda113f5e4..fc619866be 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -61,10 +61,17 @@ __BEGIN_DECLS #define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output" #define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0" +#include +#define pwm_output_values output_pwm_s + +#ifndef PWM_OUTPUT_MAX_CHANNELS + #define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS +#endif + /** * Maximum number of PWM output channels supported by the device. */ -#define PWM_OUTPUT_MAX_CHANNELS 16 +//#define PWM_OUTPUT_MAX_CHANNELS 16 /** * Lowest minimum PWM in us @@ -107,19 +114,6 @@ __BEGIN_DECLS */ typedef uint16_t servo_position_t; -/** - * Servo output status structure. - * - * May be published to output_pwm, or written to a PWM output - * device. - */ -struct pwm_output_values { - /** desired pulse widths for each of the supported channels */ - servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - unsigned channel_count; -}; - - /** * RC config values for a channel * @@ -136,11 +130,6 @@ struct pwm_output_rc_config { bool rc_reverse; }; -/* - * ORB tag for PWM outputs. - */ -ORB_DECLARE(output_pwm); - /* * ioctl() definitions * From b9face97660482faa6d41aac8c7d7197ca6155ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:47:24 +0200 Subject: [PATCH 033/136] PWM in / RC in driver: Move to generated uORB topic --- msg/input_rc.msg | 21 +++++++++ src/drivers/drv_rc_input.h | 91 ++------------------------------------ 2 files changed, 24 insertions(+), 88 deletions(-) create mode 100644 msg/input_rc.msg diff --git a/msg/input_rc.msg b/msg/input_rc.msg new file mode 100644 index 0000000000..c07d229503 --- /dev/null +++ b/msg/input_rc.msg @@ -0,0 +1,21 @@ +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_publication # publication time +uint64 timestamp_last_signal # last valid reception time +uint32 channel_count # number of channels actually being seen +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index b82ced3845..b08f10d465 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -57,11 +57,6 @@ */ #define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" -/** - * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. - */ -#define RC_INPUT_MAX_CHANNELS 18 - /** * Maximum RSSI value */ @@ -82,10 +77,9 @@ */ #define RC_INPUT_MAX_DEADZONE_US 500 -/** - * @addtogroup topics - * @{ - */ +#include +#define pwm_output_values output_pwm_s +#define rc_input_values input_rc_s /** * Input signal type, value is a control position from zero to 100 @@ -93,85 +87,6 @@ */ typedef uint16_t rc_input_t; -enum RC_INPUT_SOURCE { - RC_INPUT_SOURCE_UNKNOWN = 0, - RC_INPUT_SOURCE_PX4FMU_PPM, - RC_INPUT_SOURCE_PX4IO_PPM, - RC_INPUT_SOURCE_PX4IO_SPEKTRUM, - RC_INPUT_SOURCE_PX4IO_SBUS, - RC_INPUT_SOURCE_PX4IO_ST24, - RC_INPUT_SOURCE_MAVLINK -}; - -/** - * R/C input status structure. - * - * Published to input_rc, may also be published to other names depending - * on the board involved. - */ -struct rc_input_values { - /** publication time */ - uint64_t timestamp_publication; - - /** last valid reception time */ - uint64_t timestamp_last_signal; - - /** number of channels actually being seen */ - uint32_t channel_count; - - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */ - int32_t rssi; - - /** - * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. - * Only the true state is reliable, as there are some (PPM) receivers on the market going - * into failsafe without telling us explicitly. - * */ - bool rc_failsafe; - - /** - * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. - * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. - * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. - * */ - bool rc_lost; - - /** - * Number of lost RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_lost_frame_count; - - /** - * Number of total RC frames. - * Note: intended purpose: observe the radio link quality if RSSI is not available - * This value must not be used to trigger any failsafe-alike funtionality. - * */ - uint16_t rc_total_frame_count; - - /** - * Length of a single PPM frame. - * Zero for non-PPM systems - */ - uint16_t rc_ppm_frame_length; - - /** Input source */ - enum RC_INPUT_SOURCE input_source; - - /** measured pulse widths for each of the supported channels */ - rc_input_t values[RC_INPUT_MAX_CHANNELS]; -}; - -/** - * @} - */ - -/* - * ObjDev tag for R/C inputs. - */ -ORB_DECLARE(input_rc); - #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ From f8354bcd2641dc670c2742b56a9bb3aba42c5a9a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:47:41 +0200 Subject: [PATCH 034/136] Flow: Remove spurious orb_declare --- src/drivers/drv_px4flow.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 63cfe9816a..90bad2f25d 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,11 +46,6 @@ #define PX4FLOW0_DEVICE_PATH "/dev/px4flow0" -/* - * ObjDev tag for px4flow data. - */ -ORB_DECLARE(optical_flow); - /* * ioctl() definitions * From b71910e1044343ae8256eb6aee83d2d7b77a0381 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:47:57 +0200 Subject: [PATCH 035/136] Range finder driver: Remove spurious orb_declare --- src/drivers/drv_range_finder.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 725835cc0a..7d27c7f33a 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,11 +48,6 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus -/* - * ObjDev tag for distance sensor data. - */ -ORB_DECLARE(distance_sensor); - /* * ioctl() definitions * From 48b5a1a3fd5615dc26661ae886a867244898b3b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:48:33 +0200 Subject: [PATCH 036/136] FMU driver: Update to uORB changes --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9fad2978ca..0010e6af5e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -607,7 +607,7 @@ PX4FMU::task_main() orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif /* initialize PWM limit lib */ From b23d41e90f053e6db5ef8de829c9587f4a7f2138 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:48:45 +0200 Subject: [PATCH 037/136] IO driver: Update to uORB changes --- src/drivers/px4io/px4io.cpp | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cdfce1e9c8..e53bdbd0a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -672,8 +672,8 @@ PX4IO::init() return -1; } - if (_max_rc_input > RC_INPUT_MAX_CHANNELS) - _max_rc_input = RC_INPUT_MAX_CHANNELS; + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) + _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); @@ -1675,10 +1675,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ - input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); - uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + uint16_t regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog]; /* * Read the channel count and the first 9 channels. @@ -1697,8 +1697,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) channel_count = regs[PX4IO_P_RAW_RC_COUNT]; /* limit the channel count */ - if (channel_count > RC_INPUT_MAX_CHANNELS) { - channel_count = RC_INPUT_MAX_CHANNELS; + if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } _rc_chan_count = channel_count; @@ -1736,7 +1736,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) } /* get RSSI from input channel */ - if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / ((_rssi_pwm_max - _rssi_pwm_min) / 100); rssi = rssi > 100 ? 100 : rssi; @@ -1764,19 +1764,19 @@ PX4IO::io_publish_raw_rc() /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; /* only keep publishing RC input if we ever got a valid input */ if (_rc_last_valid == 0) { @@ -2660,19 +2660,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) /* sort out the source of the values */ if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; } /* read raw R/C input values */ @@ -2757,7 +2757,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= RC_INPUT_MAX_CHANNELS) { + if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) { /* fail with error */ return -E2BIG; } From f69480fbc3c6d2288017f593d02628399a4b849c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:48:56 +0200 Subject: [PATCH 038/136] MAVLink app: Update to uORB changes --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 55c4597677..24d0940477 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1050,7 +1050,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; - rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; /* channels */ @@ -1094,7 +1094,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_lost_frame_count = 0; rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; - rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; /* roll */ From 511fc5d42a1801de0b25d4366f805b859d6742e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:49:06 +0200 Subject: [PATCH 039/136] Sensors app: Update to uORB changes --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2626b9a715..cd5db542d3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -165,7 +165,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. From 1f8832d8c8faccfbfe7a35650975f64c24428eb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 10:49:22 +0200 Subject: [PATCH 040/136] Objects common: Remove last exceptions --- src/modules/uORB/objects_common.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index df56e931ca..a3a5565fcf 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -45,23 +45,23 @@ #include -#include -ORB_DEFINE(sensor_mag, struct mag_report); +#include "topics/sensor_mag.h" +ORB_DEFINE(sensor_mag, struct sensor_mag_s); -#include -ORB_DEFINE(sensor_accel, struct accel_report); +#include "topics/sensor_accel.h" +ORB_DEFINE(sensor_accel, struct sensor_accel_s); -#include -ORB_DEFINE(sensor_gyro, struct gyro_report); +#include "topics/sensor_gyro.h" +ORB_DEFINE(sensor_gyro, struct sensor_gyro_s); -#include -ORB_DEFINE(sensor_baro, struct baro_report); +#include "topics/sensor_baro.h" +ORB_DEFINE(sensor_baro, struct sensor_baro_s); -#include -ORB_DEFINE(output_pwm, struct pwm_output_values); +#include "topics/output_pwm.h" +ORB_DEFINE(output_pwm, struct output_pwm_s); -#include -ORB_DEFINE(input_rc, struct rc_input_values); +#include "topics/input_rc.h" +ORB_DEFINE(input_rc, struct input_rc_s); #include "topics/pwm_input.h" ORB_DEFINE(pwm_input, struct pwm_input_s); From 33bd30233ef99ea8e0fa8010e8dad36dd84619b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:07:11 +0200 Subject: [PATCH 041/136] Remove mission topic and move it to auto-generated --- msg/mission.msg | 3 +++ src/modules/{uORB/topics/mission.h => navigator/navigation.h} | 0 2 files changed, 3 insertions(+) create mode 100644 msg/mission.msg rename src/modules/{uORB/topics/mission.h => navigator/navigation.h} (100%) diff --git a/msg/mission.msg b/msg/mission.msg new file mode 100644 index 0000000000..ac135a4e09 --- /dev/null +++ b/msg/mission.msg @@ -0,0 +1,3 @@ +int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint32 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest diff --git a/src/modules/uORB/topics/mission.h b/src/modules/navigator/navigation.h similarity index 100% rename from src/modules/uORB/topics/mission.h rename to src/modules/navigator/navigation.h From d8690d7de31bea8e25025545e2f24cfdce4241e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:07:26 +0200 Subject: [PATCH 042/136] commander: Adjust to uORB mission changes --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fb69c7086..bac72a81ce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -108,6 +108,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" From 51293ad5542cd2904d4a9c6cb8cb4664cd16a8fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:07:40 +0200 Subject: [PATCH 043/136] navigator: Adjust to uORB mission changes --- src/modules/navigator/datalinkloss.cpp | 1 + src/modules/navigator/enginefailure.cpp | 1 + src/modules/navigator/gpsfailure.cpp | 1 + src/modules/navigator/mission.cpp | 1 + src/modules/navigator/mission_block.h | 2 ++ src/modules/navigator/navigation.h | 17 ++++------------- src/modules/navigator/navigator.h | 1 + src/modules/navigator/rcloss.cpp | 2 +- src/modules/navigator/rtl.cpp | 2 +- src/modules/navigator/rtl.h | 3 +-- 10 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7925809f12..9f26d05016 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index b0130a1f5c..0c344bff92 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 0ca69f0f75..e6097c87f3 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index de34797aa3..c988faab7d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index c850a0313c..eb17a329ee 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -43,6 +43,8 @@ #include +#include + #include #include #include diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 7363e7d968..d23a20c5ff 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -32,19 +32,18 @@ ****************************************************************************/ /** - * @file mission.h + * @file navigation.h * Definition of a mission consisting of mission items. * @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier */ -#ifndef TOPIC_MISSION_H_ -#define TOPIC_MISSION_H_ +#ifndef NAVIGATION_H_ +#define NAVIGATION_H_ #include #include -#include "../uORB.h" #define NUM_MISSIONS_SUPPORTED 256 @@ -103,15 +102,7 @@ struct mission_item_s { int actuator_value; /**< new value for selected actuator in ms 900...2000 */ }; -/** - * This topic used to notify navigator about mission changes, mission itself and new mission state - * must be stored in dataman before publication. - */ -struct mission_s { - int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ - unsigned count; /**< count of the missions stored in the dataman */ - int current_seq; /**< default -1, start at the one changed latest */ -}; +#include /** * @} diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 093e1be3c2..1ddedddff4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -45,6 +45,7 @@ #include #include +#include #include #include diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 40bf7f0223..fcc00148f8 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b75b7fa221..246007ad81 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -47,7 +47,7 @@ #include #include -#include +#include #include #include "navigator.h" diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 5928f1f07b..3506065802 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,8 +44,7 @@ #include #include -#include -#include +#include #include #include From cb7a4f57999d8f7208b3be651dcfa9e8df992c41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:07:53 +0200 Subject: [PATCH 044/136] dataman: Adjust to uORB mission changes --- src/modules/dataman/dataman.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index ad8df4970b..7ea93b87e9 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -39,6 +39,7 @@ #ifndef _DATAMANAGER_H #define _DATAMANAGER_H +#include #include #include From a0b20f223cb92c4996de67ce4c81071b3275a1e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:08:05 +0200 Subject: [PATCH 045/136] MAVLink: Adjust to uORB mission changes --- src/modules/mavlink/mavlink_mission.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 3cf0323a05..2c1a3d88f6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include From bc216d59c084d4b560865b8a8fa3117ff69d6bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:08:32 +0200 Subject: [PATCH 046/136] uORB: Keep mission in objects common until generator is up to date --- src/modules/uORB/objects_common.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a3a5565fcf..42b1481682 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -140,6 +140,9 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" +// XXX onboard and offboard mission are still declared here until this is +// generator supported +#include ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); From e784cac786be4c0493e406c71038049848ea242e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 20 Aug 2015 11:33:25 +0200 Subject: [PATCH 047/136] Fix upper / lowercase typo in uORB topic names in driver headers --- src/drivers/drv_accel.h | 2 +- src/drivers/drv_baro.h | 2 +- src/drivers/drv_gyro.h | 2 +- src/drivers/drv_mag.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index f17d1ac9c2..3af204edcd 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -51,7 +51,7 @@ #define ACCEL1_DEVICE_PATH "/dev/accel1" #define ACCEL2_DEVICE_PATH "/dev/accel2" -#include +#include #define accel_report sensor_accel_s /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 4da8e8050f..87bf1000ec 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -50,7 +50,7 @@ #define BARO_BASE_DEVICE_PATH "/dev/baro" #define BARO0_DEVICE_PATH "/dev/baro0" -#include +#include #define baro_report sensor_baro_s /* diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index feaa38881f..2ed65802cd 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -51,7 +51,7 @@ #define GYRO1_DEVICE_PATH "/dev/gyro1" #define GYRO2_DEVICE_PATH "/dev/gyro2" -#include +#include #define gyro_report sensor_gyro_s /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3abe84c883..75d914de8e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -49,7 +49,7 @@ #define MAG1_DEVICE_PATH "/dev/mag1" #define MAG2_DEVICE_PATH "/dev/mag2" -#include +#include #define mag_report sensor_mag_s /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ From dc0f8e2ab19c3417c535d1b55811eb54a8b5ca8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Aug 2015 14:33:54 +0200 Subject: [PATCH 048/136] Added ORB_DEFINE for mission topic, which is needed by the orb listener module --- src/modules/uORB/objects_common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 42b1481682..ac98048771 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -140,6 +140,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" +ORB_DEFINE(mission, struct mission_s); // XXX onboard and offboard mission are still declared here until this is // generator supported #include From 9526f7c677656ce3833e7be6a98aa7a4bf586fe6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 21 Aug 2015 06:08:46 -0600 Subject: [PATCH 049/136] fix rc_input_s ref --- .cproject | 4 ++-- src/drivers/px4fmu/fmu.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.cproject b/.cproject index f90fe5d114..a4bd673686 100644 --- a/.cproject +++ b/.cproject @@ -18,7 +18,7 @@ - + @@ -77,7 +77,7 @@ - + diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0010e6af5e..c7b29a1066 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -772,8 +772,8 @@ PX4FMU::task_main() // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + if (rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } for (uint8_t i = 0; i < rc_in.channel_count; i++) { From d08f4c747ee35b3f08567a4d1c7f07c81ae09c20 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 21 Aug 2015 06:12:49 -0600 Subject: [PATCH 050/136] revert Eclipse project change --- .cproject | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.cproject b/.cproject index a4bd673686..a0577f3329 100644 --- a/.cproject +++ b/.cproject @@ -18,7 +18,7 @@ - + From 7c33e1fdb5c89b192586f7d0ba406d07431d9c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:51:28 +0200 Subject: [PATCH 051/136] Update NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 678eea3d2c..8891d035df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4 +Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee From a38d954d9afc08984b747af45fab1684c5e57650 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:52:05 +0200 Subject: [PATCH 052/136] Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index d23a551e10..c390a63abf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit d23a551e108d92c7a49a66d162cb9d542bbe6be1 +Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118 From 9a6a6fa6fb89d7729ca30a72c1a2e9e847e2b2b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:52:21 +0200 Subject: [PATCH 053/136] Update NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 8004677d8c..8891d035df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 8004677d8c9222685f331409469e78a66375d55f +Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee From 0d8de2b84d94999c551d0a45bfe21987b2cae769 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:52:29 +0200 Subject: [PATCH 054/136] Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index e35b4c8e51..c390a63abf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit e35b4c8e5171b2ee4a37ae0d6f240c887c455227 +Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118 From 4ec16efe686afbad267f58e7b859fa6638f46afb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 22:58:09 +0300 Subject: [PATCH 055/136] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 46793c5e06..181b4f6094 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 +Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 From 473d321af8bead4bd45299ad723b4c7beec94bce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:04:41 +0300 Subject: [PATCH 056/136] UAVCAN Magnetometer driver update - no logical changes --- src/modules/uavcan/sensors/mag.cpp | 10 +++++----- src/modules/uavcan/sensors/mag.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index d08f5ed32b..ef9613b1f7 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -45,7 +45,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why? _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; @@ -141,16 +141,16 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; unlock(); publish(msg.getSrcNodeID().get(), &_report); diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index b74afeb0be..a13883a494 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -40,7 +40,7 @@ #include "sensor_bridge.hpp" #include -#include +#include class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { @@ -57,14 +57,14 @@ private: ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, void (UavcanMagnetometerBridge::*) - (const uavcan::ReceivedDataStructure &) > + (const uavcan::ReceivedDataStructure &) > MagCbBinder; - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; mag_report _report = {}; }; From 3860dbdc8cfec81971f709b696b6d1f3a7a8ca8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:07:16 +0300 Subject: [PATCH 057/136] UAVCAN Barometer driver fix - reporting temperature in Celsius degrees, not in Kelvin --- src/modules/uavcan/sensors/baro.cpp | 8 +++----- src/modules/uavcan/sensors/baro.hpp | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8198d1d994..50655d9180 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -46,9 +46,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : _sub_air_pressure_data(node), _sub_air_temperature_data(node), _reports(nullptr) -{ - last_temperature = 0.0f; -} +{ } int UavcanBarometerBridge::init() { @@ -154,7 +152,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { - last_temperature = msg.static_temperature; + last_temperature_kelvin = msg.static_temperature; } void UavcanBarometerBridge::air_pressure_sub_cb(const @@ -163,7 +161,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = last_temperature; + report.temperature = last_temperature_kelvin - 273.15F; 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 98dc5cf42f..4024cb156b 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -77,6 +77,6 @@ private: uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; RingBuffer *_reports; - float last_temperature; + float last_temperature_kelvin = 0.0; }; From 9b32a55b402d9c1d9a6bce716c1a03f2b2c1e1cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Aug 2015 14:58:26 +0300 Subject: [PATCH 058/136] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 181b4f6094..bfee82f3c4 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 +Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 From 4296dac884220d2a255ff02b5c1a87cd0a19c4a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:06:26 +0300 Subject: [PATCH 059/136] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index bfee82f3c4..b317b94d98 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 +Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 From 6864779fa6bc3e895461d47e1322f4e90c642d7a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:51:36 +0300 Subject: [PATCH 060/136] Multiple stages for UAVCAN_ENABLE --- ROMFS/px4fmu_common/init.d/rc.uavcan | 30 ++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_params.c | 7 +++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 08ba86d789..9f38ff90bc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,7 +3,26 @@ # UAVCAN initialization script. # +# +# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable. +# TODO there should be a smarter way. +# +set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 +then + set UAVCAN_ENABLE 1 +fi +if param compare UAVCAN_ENABLE 2 +then + set UAVCAN_ENABLE 2 +fi + +echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" + +# +# Starting stuff according to UAVCAN_ENABLE value +# +if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then @@ -16,3 +35,14 @@ then tone_alarm $TUNE_ERR fi fi + +if [ $UAVCAN_ENABLE -ge 2 ] +then + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi +fi diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index e6ea8a8fb7..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -41,10 +41,13 @@ /** * Enable UAVCAN. * - * Enables support for UAVCAN-interfaced actuators and sensors. + * Allowed values: + * 0 - UAVCAN disabled. + * 1 - Enabled support for UAVCAN actuators and sensors. + * 2 - Enabled support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 1 + * @max 2 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); From e1f33871efd87f4b910390c853a9d301aaab8028 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 17:25:23 +0300 Subject: [PATCH 061/136] UAVCAN enabled by default --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 934263b6b4..76d0e8ea09 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); /** * UAVCAN Node ID. From 0e10638c7e0d43c6ed4d608ee418e759b0b77202 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 23:11:12 +0300 Subject: [PATCH 062/136] Temporary fix to UAVCAN sensor prioritization issue, see #2081, #2715 --- ROMFS/px4fmu_common/init.d/rc.uavcan | 25 +++++++++++++------------ ROMFS/px4fmu_common/init.d/rcS | 9 +++++---- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 9f38ff90bc..3615c177de 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -10,11 +10,11 @@ set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 then - set UAVCAN_ENABLE 1 + set UAVCAN_ENABLE 1 fi if param compare UAVCAN_ENABLE 2 then - set UAVCAN_ENABLE 2 + set UAVCAN_ENABLE 2 fi echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" @@ -26,9 +26,6 @@ if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs - sleep 1 echo "[i] UAVCAN started" else echo "[i] ERROR: Could not start UAVCAN" @@ -38,11 +35,15 @@ fi if [ $UAVCAN_ENABLE -ge 2 ] then - if uavcan start fw - then - echo "[i] UAVCAN servers started" - else - echo "[i] ERROR: Could not start UAVCAN servers" - tone_alarm $TUNE_ERR - fi + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi fi + +# First sensor publisher to initialize takes lowest instance ID +# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs +sleep 8 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c1c8dc303d..b21ca9ec88 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -299,6 +299,11 @@ then then fi + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors System (start before Commander so Preflight checks are properly run) # @@ -491,10 +496,6 @@ then fi fi - # UAVCAN - # - sh /etc/init.d/rc.uavcan - # # Logging # From 281cf49586f28434880ba422e03ef3b3e7567c3e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 05:45:00 +0300 Subject: [PATCH 063/136] Libuavcan update - fixed file IO loops --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index b317b94d98..cf39ecf879 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 +Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e From 59fd94da3e75dc8a827979a5f19ca93f88efa4a7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 12:37:51 +0300 Subject: [PATCH 064/136] Libuavcan synced with the version that has DSDL v1.0.0rc1 - no logical changes --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index cf39ecf879..3ae5400aa5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e +Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd From 08fde4f505a8ae085fb49572add2028331dc8998 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 20:19:02 +0300 Subject: [PATCH 065/136] UAVCAN startup delay fix --- ROMFS/px4fmu_common/init.d/rc.uavcan | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 3615c177de..37e6893bd1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -44,6 +44,9 @@ then fi fi -# First sensor publisher to initialize takes lowest instance ID -# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs -sleep 8 +if [ $UAVCAN_ENABLE -ge 1 ] +then + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs + sleep 8 +fi From f371ece43b5e6b028a4be4c90c06136adf087e22 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 22:56:49 +0300 Subject: [PATCH 066/136] UAVCAN_ENABLE defaults to 0 --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 76d0e8ea09..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); /** * UAVCAN Node ID. From 221c222ca257862aec6876ce1774cb690e13150d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:13 +0200 Subject: [PATCH 067/136] comment out code which relied on old tecs logging format --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index cfba54d452..c07956fd6e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; - _status.flightPathAngleFiltered = flightPathAngleFiltered; - _status.airspeedDerivativeSp = airspeedDerivativeSp; - _status.airspeedDerivative = airspeedDerivative; - _status.totalEnergyRateSp = totalEnergyRateSp; - _status.totalEnergyRate = totalEnergyRate; - _status.energyDistributionRateSp = energyDistributionRateSp; - _status.energyDistributionRate = energyDistributionRate; - _status.mode = mode; + // /* Write part of the status message */ + // _status.flightPathAngleSp = flightPathAngleSp; + // _status.flightPathAngle = flightPathAngle; + // _status.flightPathAngleFiltered = flightPathAngleFiltered; + // _status.airspeedDerivativeSp = airspeedDerivativeSp; + // _status.airspeedDerivative = airspeedDerivative; + // _status.totalEnergyRateSp = totalEnergyRateSp; + // _status.totalEnergyRate = totalEnergyRate; + // _status.energyDistributionRateSp = energyDistributionRateSp; + // _status.energyDistributionRate = energyDistributionRate; + // _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ From 6cdceb1829d2674c27a9e53639badd9c901fe03d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:42 +0200 Subject: [PATCH 068/136] fix tecs status logging --- msg/tecs_status.msg | 11 +++-- src/lib/external_lgpl/tecs/tecs.cpp | 49 +++++++++++-------- src/lib/external_lgpl/tecs/tecs.h | 41 +++++++++++----- .../fw_pos_control_l1_main.cpp | 29 ++++++----- src/modules/sdlog2/sdlog2.c | 11 +++-- src/modules/sdlog2/sdlog2_messages.h | 14 +++--- 6 files changed, 94 insertions(+), 61 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index ccac921e2b..7e9d6670c9 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -18,9 +18,12 @@ float32 airspeed_filtered float32 airspeedDerivativeSp float32 airspeedDerivative -float32 totalEnergyRateSp -float32 totalEnergyRate -float32 energyDistributionRateSp -float32 energyDistributionRate +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_sp +float32 pitch_sp uint8 mode diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6f049d4f52..9e47590fd2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); - float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + _STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement - STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; - _STEdotErrLast = STEdot_error; + _STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = _STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle @@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later - _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle @@ -438,11 +438,11 @@ void TECS::_update_pitch(void) // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); - float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded - float integ7_input = SEB_error * _integGain; + float integ7_input = _SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); @@ -460,7 +460,7 @@ void TECS::_update_pitch(void) // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); - float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; if (_climbOutDem) { temp += _PITCHminf * gainInv; @@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.mode = ECL_TECS_MODE_NORMAL; } - _tecs_state.hgt_dem = _hgt_dem_adj; - _tecs_state.hgt = _integ3_state; - _tecs_state.dhgt_dem = _hgt_rate_dem; - _tecs_state.dhgt = _integ2_state; - _tecs_state.spd_dem = _TAS_dem_adj; - _tecs_state.spd = _integ5_state; - _tecs_state.dspd = _vel_dot; - _tecs_state.ithr = _integ6_state; - _tecs_state.iptch = _integ7_state; - _tecs_state.thr = _throttle_dem; - _tecs_state.ptch = _pitch_dem; - _tecs_state.dspd_dem = _TAS_rate_dem; + _tecs_state.altitude_sp = _hgt_dem_adj; + _tecs_state.altitude_filtered = _integ3_state; + _tecs_state.altitude_rate_sp = _hgt_rate_dem; + _tecs_state.altitude_rate = _integ2_state; + + _tecs_state.airspeed_sp = _TAS_dem_adj; + _tecs_state.airspeed_rate_sp = _TAS_rate_dem; + _tecs_state.airspeed_filtered = _integ5_state; + _tecs_state.airspeed_rate = _vel_dot; + + _tecs_state.total_energy_error = _STE_error; + _tecs_state.energy_distribution_error = _SEB_error; + _tecs_state.total_energy_rate_error = _STEdot_error; + _tecs_state.energy_distribution_error = _SEBdot_error; + + _tecs_state.energy_error_integ = _integ6_state; + _tecs_state.energy_distribution_error_integ = _integ7_state; + + + _tecs_state.throttle_sp = _throttle_dem; + _tecs_state.pitch_sp = _pitch_dem; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 81a5e2d691..0fbda0fe9a 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -79,6 +79,10 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), + _STE_error(0.0f), + _STEdot_error(0.0f), + _SEB_error(0.0f), + _SEBdot_error(0.0f), _airspeed_enabled(false), _states_initalized(false), _in_air(false), @@ -137,18 +141,22 @@ public: struct tecs_state { uint64_t timestamp; - float hgt; - float dhgt; - float hgt_dem; - float dhgt_dem; - float spd_dem; - float spd; - float dspd; - float ithr; - float iptch; - float thr; - float ptch; - float dspd_dem; + float altitude_filtered; + float altitude_sp; + float altitude_rate; + float altitude_rate_sp; + float airspeed_filtered; + float airspeed_sp; + float airspeed_rate; + float airspeed_rate_sp; + float energy_error_integ; + float energy_distribution_error_integ; + float total_energy_error; + float total_energy_rate_error; + float energy_distribution_error; + float energy_distribution_rate_error; + float throttle_sp; + float pitch_sp; enum ECL_TECS_MODE mode; }; @@ -376,6 +384,15 @@ private: // Specific energy error quantities float _STE_error; + // Energy error rate + float _STEdot_error; + + // Specific energy balance error + float _SEB_error; + + // Specific energy balance error rate + float _SEBdot_error; + // Time since last update of main TECS loop (seconds) float _DT; 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 ceea71fca9..e14b84fb39 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 @@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ break; } - t.altitudeSp = s.hgt_dem; - t.altitude_filtered = s.hgt; - t.airspeedSp = s.spd_dem; - t.airspeed_filtered = s.spd; + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; - t.flightPathAngleSp = s.dhgt_dem; - t.flightPathAngle = s.dhgt; - t.flightPathAngleFiltered = s.dhgt; + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; - t.airspeedDerivativeSp = s.dspd_dem; - t.airspeedDerivative = s.dspd; + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; - t.totalEnergyRateSp = s.thr; - t.totalEnergyRate = s.ithr; - t.energyDistributionRateSp = s.ptch; - t.energyDistributionRate = s.iptch; + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_sp = s.throttle_sp; + t.pitch_sp = s.pitch_sp; if (_tecs_status_pub > 0) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 71ac96350a..c1c01ec06e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1863,15 +1863,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; - log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; - log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; - log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; + log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; + log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp; + log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e75b6ca256..1363b37ba7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -360,16 +360,16 @@ struct log_TECS_s { float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; - float flightPathAngleFiltered; float airspeedSp; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; + float totalEnergyError; + float totalEnergyRateError; + float energyDistributionError; + float energyDistributionRateError; + float throttle_sp; + float pitch_sp; uint8_t mode; }; @@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), From 8cda0ec26669302b2e06513b9595b7a78c2257db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jul 2015 17:07:13 +0200 Subject: [PATCH 069/136] EKF memory usage fix --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 2 files changed, 2 insertions(+), 2 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 07b8c8dfd5..4588f3c901 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 @@ -1115,7 +1115,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 7500, + 4800, (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index f519621137..2a582ebbb8 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_22states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400 From 7cb97f1d39e3e50f3d2673dc2641a6c206938aa2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 14:20:21 +0200 Subject: [PATCH 070/136] top: Fix code style --- src/systemcmds/top/top.c | 123 +++++++++++++++++++++++---------------- 1 file changed, 73 insertions(+), 50 deletions(-) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 37e913040d..f82b4bda58 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 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 @@ -36,8 +35,8 @@ * @file top.c * Tool similar to UNIX top command * @see http://en.wikipedia.org/wiki/Top_unix - * - * @author Lorenz Meier + * + * @author Lorenz Meier */ #include @@ -64,23 +63,40 @@ static const char * tstate_name(const tstate_t s) { switch (s) { - case TSTATE_TASK_INVALID: return "init"; + case TSTATE_TASK_INVALID: + return "init"; - case TSTATE_TASK_PENDING: return "PEND"; - case TSTATE_TASK_READYTORUN: return "READY"; - case TSTATE_TASK_RUNNING: return "RUN"; + case TSTATE_TASK_PENDING: + return "PEND"; - case TSTATE_TASK_INACTIVE: return "inact"; - case TSTATE_WAIT_SEM: return "w:sem"; + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; #ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: return "w:sig"; + + case TSTATE_WAIT_SIG: + return "w:sig"; #endif #ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe"; - case TSTATE_WAIT_MQNOTFULL: return "w:mqf"; + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; #endif #ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: return "w:pgf"; + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; #endif default: @@ -102,8 +118,9 @@ top_main(void) uint64_t last_times[CONFIG_MAX_TASKS]; float curr_loads[CONFIG_MAX_TASKS]; - for (int t = 0; t < CONFIG_MAX_TASKS; t++) + for (int t = 0; t < CONFIG_MAX_TASKS; t++) { last_times[t] = 0; + } float interval_time_ms_inv = 0.f; @@ -118,8 +135,9 @@ top_main(void) curr_time_us = hrt_absolute_time(); idle_time_us = system_load.tasks[0].total_runtime; - if (new_time > interval_start_time) + if (new_time > interval_start_time) { interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + } running_count = 0; blocked_count = 0; @@ -155,19 +173,22 @@ top_main(void) } interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; + system_load.tasks[i].total_runtime > last_times[i]) + ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 + : 0; last_times[i] = system_load.tasks[i].total_runtime; if (system_load.tasks[i].valid && (new_time > interval_start_time)) { curr_loads[i] = interval_runtime * interval_time_ms_inv; - if (i > 0) + if (i > 0) { total_user_time += interval_runtime; - } else + } + + } else { curr_loads[i] = 0; + } } for (i = 0; i < CONFIG_MAX_TASKS; i++) { @@ -182,63 +203,65 @@ top_main(void) /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) + if (task_load > (1.f - idle)) { task_load = (1.f - idle); + } sched_load = 1.f - idle - task_load; /* print system information */ printf("\033[H"); /* move cursor home and clear screen */ printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); + system_load.total_count, + running_count, + blocked_count); printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d); /* header for task list */ printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", #if CONFIG_RR_INTERVAL > 0 - "TSLICE" + "TSLICE" #else - "STATE" + "STATE" #endif - ); + ); } unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; unsigned stack_free = 0; uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) + if (*stack_sweeper++ != 0xff) { break; + } stack_free++; } printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); #if CONFIG_RR_INTERVAL > 0 /* print scheduling info with RR time slice */ From b70c9a84c621af6d0a48c0f1ba99e9385426e4ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 15:22:28 +0200 Subject: [PATCH 071/136] systemlib: Add ability to dump memory usage --- src/modules/systemlib/module.mk | 1 + src/modules/systemlib/printload.c | 278 ++++++++++++++++++++++++++++++ src/modules/systemlib/printload.h | 59 +++++++ src/systemcmds/top/top.c | 230 ++---------------------- 4 files changed, 354 insertions(+), 214 deletions(-) create mode 100644 src/modules/systemlib/printload.c create mode 100644 src/modules/systemlib/printload.h diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..b531330ab3 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -42,6 +42,7 @@ SRCS = err.c \ bson/tinybson.c \ conversions.c \ cpuload.c \ + printload.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c new file mode 100644 index 0000000000..e40791fe89 --- /dev/null +++ b/src/modules/systemlib/printload.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file printload.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +static const char * +tstate_name(const tstate_t s) +{ + switch (s) { + case TSTATE_TASK_INVALID: + return "init"; + + case TSTATE_TASK_PENDING: + return "PEND"; + + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; +#ifndef CONFIG_DISABLE_SIGNALS + + case TSTATE_WAIT_SIG: + return "w:sig"; +#endif +#ifndef CONFIG_DISABLE_MQUEUE + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; +#endif +#ifdef CONFIG_PAGING + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; +#endif + + default: + return "ERROR"; + } +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ + print_state->new_time = t; + + int i; + uint64_t curr_time_us; + uint64_t idle_time_us; + char *clear_line = '\0'; + + /* print system information */ + if (fd == 1) { + dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + clear_line = CL; + } + + curr_time_us = t; + idle_time_us = system_load.tasks[0].total_runtime; + + if (print_state->new_time > print_state->interval_start_time) { + print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000)); + } + + print_state->running_count = 0; + print_state->blocked_count = 0; + print_state->total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime; + + if (system_load.tasks[i].valid) { + switch (system_load.tasks[i].tcb->task_state) { + case TSTATE_TASK_PENDING: + case TSTATE_TASK_READYTORUN: + case TSTATE_TASK_RUNNING: + print_state->running_count++; + break; + + case TSTATE_TASK_INVALID: + case TSTATE_TASK_INACTIVE: + case TSTATE_WAIT_SEM: +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: + case TSTATE_WAIT_MQNOTFULL: +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: +#endif + print_state->blocked_count++; + break; + } + } + + interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 && + system_load.tasks[i].total_runtime > print_state->last_times[i]) + ? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000 + : 0; + + print_state->last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv; + + if (i > 0) { + print_state->total_user_time += interval_runtime; + } + + } else { + print_state->curr_loads[i] = 0; + } + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle; + float task_load; + float sched_load; + + idle = print_state->curr_loads[0]; + task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv; + + /* this can happen if one tasks total runtime was not computed + correctly by the scheduler instrumentation TODO */ + if (task_load > (1.f - idle)) { + task_load = (1.f - idle); + } + + sched_load = 1.f - idle - task_load; + + dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n", + clear_line, + system_load.total_count, + print_state->running_count, + print_state->blocked_count); + dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", + clear_line, + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); + dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n", + clear_line, + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d, + clear_line); + + /* header for task list */ + dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n", + clear_line, + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", +#if CONFIG_RR_INTERVAL > 0 + "TSLICE" +#else + "STATE" +#endif + ); + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) { + break; + } + + stack_free++; + } + + dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", + clear_line, + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(print_state->curr_loads[i] * 100.0f), + (int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); + +#if CONFIG_RR_INTERVAL > 0 + /* print scheduling info with RR time slice */ + dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice); +#else + // print task state instead + dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); +#endif + } + } + + print_state->interval_start_time = print_state->new_time; +} + diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h new file mode 100644 index 0000000000..f0719d2106 --- /dev/null +++ b/src/modules/systemlib/printload.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file printload.h + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include + +__EXPORT struct print_load_s { + uint64_t total_user_time; + + int running_count; + int blocked_count; + + uint64_t new_time; + uint64_t interval_start_time; + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + float interval_time_ms_inv; +}; + +__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); + +__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index f82b4bda58..a4846885c5 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -48,232 +48,34 @@ #include #include +#include #include -#define CL "\033[K" // clear line - /** * Start the top application. */ -__EXPORT int top_main(void); - -extern struct system_load_s system_load; - -static const char * -tstate_name(const tstate_t s) -{ - switch (s) { - case TSTATE_TASK_INVALID: - return "init"; - - case TSTATE_TASK_PENDING: - return "PEND"; - - case TSTATE_TASK_READYTORUN: - return "READY"; - - case TSTATE_TASK_RUNNING: - return "RUN"; - - case TSTATE_TASK_INACTIVE: - return "inact"; - - case TSTATE_WAIT_SEM: - return "w:sem"; -#ifndef CONFIG_DISABLE_SIGNALS - - case TSTATE_WAIT_SIG: - return "w:sig"; -#endif -#ifndef CONFIG_DISABLE_MQUEUE - - case TSTATE_WAIT_MQNOTEMPTY: - return "w:mqe"; - - case TSTATE_WAIT_MQNOTFULL: - return "w:mqf"; -#endif -#ifdef CONFIG_PAGING - - case TSTATE_WAIT_PAGEFILL: - return "w:pgf"; -#endif - - default: - return "ERROR"; - } -} +__EXPORT int top_main(int argc, char *argv[]); int -top_main(void) +top_main(int argc, char *argv[]) { - uint64_t total_user_time = 0; + hrt_abstime curr_time = hrt_absolute_time(); - int running_count = 0; - int blocked_count = 0; - - uint64_t new_time = hrt_absolute_time(); - uint64_t interval_start_time = new_time; - - uint64_t last_times[CONFIG_MAX_TASKS]; - float curr_loads[CONFIG_MAX_TASKS]; - - for (int t = 0; t < CONFIG_MAX_TASKS; t++) { - last_times[t] = 0; - } - - float interval_time_ms_inv = 0.f; + struct print_load_s load; + init_print_load_s(curr_time, &load); /* clear screen */ - printf("\033[2J"); + dprintf(1, "\033[2J\n"); + + if (argc > 1 && !strcmp(argv[1], "once")) { + print_load(curr_time, 1, &load); + sleep(1); + print_load(hrt_absolute_time(), 1, &load); + return 0; + } for (;;) { - int i; - uint64_t curr_time_us; - uint64_t idle_time_us; - - curr_time_us = hrt_absolute_time(); - idle_time_us = system_load.tasks[0].total_runtime; - - if (new_time > interval_start_time) { - interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); - } - - running_count = 0; - blocked_count = 0; - total_user_time = 0; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime; - - if (system_load.tasks[i].valid) { - switch (system_load.tasks[i].tcb->task_state) { - case TSTATE_TASK_PENDING: - case TSTATE_TASK_READYTORUN: - case TSTATE_TASK_RUNNING: - running_count++; - break; - - case TSTATE_TASK_INVALID: - case TSTATE_TASK_INACTIVE: - case TSTATE_WAIT_SEM: -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: - case TSTATE_WAIT_MQNOTFULL: -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: -#endif - blocked_count++; - break; - } - } - - interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; - - last_times[i] = system_load.tasks[i].total_runtime; - - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - curr_loads[i] = interval_runtime * interval_time_ms_inv; - - if (i > 0) { - total_user_time += interval_runtime; - } - - } else { - curr_loads[i] = 0; - } - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - if (system_load.tasks[i].tcb->pid == 0) { - float idle; - float task_load; - float sched_load; - - idle = curr_loads[0]; - task_load = (float)(total_user_time) * interval_time_ms_inv; - - /* this can happen if one tasks total runtime was not computed - correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) { - task_load = (1.f - idle); - } - - sched_load = 1.f - idle - task_load; - - /* print system information */ - printf("\033[H"); /* move cursor home and clear screen */ - printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); - printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); - printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); - - /* header for task list */ - printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", -#if CONFIG_RR_INTERVAL > 0 - "TSLICE" -#else - "STATE" -#endif - ); - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) { - break; - } - - stack_free++; - } - - printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); - -#if CONFIG_RR_INTERVAL > 0 - /* print scheduling info with RR time slice */ - printf(" %6d\n", system_load.tasks[i].tcb->timeslice); -#else - // print task state instead - printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); -#endif - } - } - - interval_start_time = new_time; + print_load(curr_time, 1, &load); /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { @@ -302,7 +104,7 @@ top_main(void) usleep(200000); } - new_time = hrt_absolute_time(); + curr_time = hrt_absolute_time(); } return OK; From c264d6bb99792fe166bf045dc28a79890137d226 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 15:27:05 +0200 Subject: [PATCH 072/136] SDLOG2: Store load / mem use post-flight --- src/modules/sdlog2/sdlog2.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c1c01ec06e..ea843f9202 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -105,6 +105,7 @@ #include #include #include +#include #include #include @@ -727,9 +728,16 @@ void sdlog2_start_log() } /* write all performance counters */ + hrt_abstime curr_time = hrt_absolute_time(); + struct print_load_s load; int perf_fd = open_perf_file("preflight"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); perf_print_all(perf_fd); + dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n"); + usleep(500 * 1000); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* reset performance counters to get in-flight min and max values in post flight log */ @@ -765,8 +773,15 @@ void sdlog2_stop_log() /* write all performance counters */ int perf_fd = open_perf_file("postflight"); + hrt_abstime curr_time = hrt_absolute_time(); dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); perf_print_all(perf_fd); + struct print_load_s load; + dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); + sleep(1); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* free log writer performance counter */ From 86e2e7e2a6d18403f063685d124880112f3e551a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 19:21:54 +0200 Subject: [PATCH 073/136] print load: Initialize to empty string, not null pointer --- src/modules/systemlib/printload.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c index e40791fe89..23dde1ce44 100644 --- a/src/modules/systemlib/printload.c +++ b/src/modules/systemlib/printload.c @@ -120,7 +120,7 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state) int i; uint64_t curr_time_us; uint64_t idle_time_us; - char *clear_line = '\0'; + char *clear_line = ""; /* print system information */ if (fd == 1) { From e1aaaa1f1896fad5b422b0b8fe3c8c1edad7625c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:31:45 +0200 Subject: [PATCH 074/136] MAVLink: Start app using pre-configured streams to save some script execution overhead. Frees 1K during app startup, which should help FMUv1 considerably. --- ROMFS/px4fmu_common/init.d/rc.usb | 17 +---------------- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 3 ++- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 027d2aca5d..4a157a683f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,22 +3,7 @@ # USB MAVLink start # -mavlink start -r 800000 -d /dev/ttyACM0 -x -# Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 -mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 -mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 -mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 -mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 -mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 -mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 -mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 -mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 -mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 -mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 +mavlink start -r 800000 -d /dev/ttyACM0 -m config -x # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f2dcd33d79..f172dfbeae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1353,6 +1353,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(optarg, "config") == 0) { + _mode = MAVLINK_MODE_CONFIG; } break; @@ -1449,8 +1451,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _task_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1537,6 +1537,23 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 5.0f); break; + case MAVLINK_MODE_CONFIG: + // Enable a number of interesting streams we want via USB + configure_stream("PARAM_VALUE", 300.0f); + configure_stream("MISSION_ITEM", 50.0f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VFR_HUD", 20.0f); + configure_stream("ATTITUDE", 100.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("MANUAL_CONTROL", 5.0f); + configure_stream("HIGHRES_IMU", 100.0f); + configure_stream("GPS_RAW_INT", 20.0f); + default: break; } @@ -1703,6 +1720,9 @@ Mavlink::task_main(int argc, char *argv[]) } perf_end(_loop_perf); + + /* confirm task running only once fully initialized */ + _task_running = true; } delete _subscribe_to_stream; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a484558579..455d9c796e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -129,7 +129,8 @@ public: MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, - MAVLINK_MODE_OSD + MAVLINK_MODE_OSD, + MAVLINK_MODE_CONFIG }; void set_mode(enum MAVLINK_MODE); From be638598936e7d0654a00384a53ac64c3654c685 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:44:29 +0200 Subject: [PATCH 075/136] FMU driver: Remove excessive task space --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f2ec0285be..8109ec7603 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -355,7 +355,7 @@ PX4FMU::init() _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1600, + 1200, (main_t)&PX4FMU::task_main_trampoline, nullptr); From 4463f1a8a2389f8f13889fc6fc33e06ccd880d71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:51:43 +0200 Subject: [PATCH 076/136] MAVLink: Add streams to C-based stream config --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1af7863618..7a6a207832 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1699,16 +1699,20 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("MISSION_ITEM", 50.0f); configure_stream("NAMED_VALUE_FLOAT", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("VFR_HUD", 20.0f); configure_stream("ATTITUDE", 100.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream("RC_CHANNELS_RAW", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream("SERVO_OUTPUT_RAW_1", 20.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("MANUAL_CONTROL", 5.0f); configure_stream("HIGHRES_IMU", 100.0f); configure_stream("GPS_RAW_INT", 20.0f); + configure_stream("CAMERA_TRIGGER", 500.0f); + configure_stream("VTOL_STATE", 2.0f); default: break; From 10047964e309035e6efc5c0e2fdf2d1d9fe5bdc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 21:12:26 +0200 Subject: [PATCH 077/136] Fix compile error on load print struct --- src/modules/systemlib/printload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index f0719d2106..2e0bfb62a8 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -41,7 +41,7 @@ #include -__EXPORT struct print_load_s { +struct __EXPORT print_load_s { uint64_t total_user_time; int running_count; From 90d8b22f4fa257be2a43caa6933c996e719b9230 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 21:20:15 +0200 Subject: [PATCH 078/136] Compile fix for POSIX for load print routines --- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/print_load_posix.c | 74 ++++++++++++++++++++++++ 2 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 src/modules/systemlib/print_load_posix.c diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 22ca969b3d..194660776e 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -60,7 +60,8 @@ SRCS += err.c \ endif ifneq ($(PX4_TARGET_OS),qurt) -SRCS += hx_stream.c +SRCS += hx_stream.c \ + print_load_posix.c endif MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c new file mode 100644 index 0000000000..ef8571a7e8 --- /dev/null +++ b/src/modules/systemlib/print_load_posix.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file printload.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ +} + From 808d817cfe14a34e48e69f9e140d6c45be3c2741 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:10:29 +0200 Subject: [PATCH 079/136] Add missing decls to print load --- src/modules/systemlib/printload.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 2e0bfb62a8..3199059004 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -39,6 +39,10 @@ * @author Lorenz Meier */ +#pragma once + +__BEGIN_DECLS + #include struct __EXPORT print_load_s { @@ -57,3 +61,5 @@ struct __EXPORT print_load_s { __EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); __EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); + +__END_DECLS From 07a88e9e866d461930534ed5ad77f03d2b70dadd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:16:03 +0200 Subject: [PATCH 080/136] print load: Fix struct visibility --- src/modules/systemlib/printload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 3199059004..09f8a562b9 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -45,7 +45,7 @@ __BEGIN_DECLS #include -struct __EXPORT print_load_s { +struct print_load_s { uint64_t total_user_time; int running_count; From 2c4e72a081b6155cfff9ff1ccda2f941069225f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:10:29 +0200 Subject: [PATCH 081/136] Add missing decls to print load --- src/modules/systemlib/printload.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 2e0bfb62a8..3199059004 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -39,6 +39,10 @@ * @author Lorenz Meier */ +#pragma once + +__BEGIN_DECLS + #include struct __EXPORT print_load_s { @@ -57,3 +61,5 @@ struct __EXPORT print_load_s { __EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); __EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); + +__END_DECLS From 6fda74dbd628cf7412077d5c6d0a180c58f8ed8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:16:03 +0200 Subject: [PATCH 082/136] print load: Fix struct visibility --- src/modules/systemlib/printload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 3199059004..09f8a562b9 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -45,7 +45,7 @@ __BEGIN_DECLS #include -struct __EXPORT print_load_s { +struct print_load_s { uint64_t total_user_time; int running_count; From 24af256516fb7cd3757916bcb5826094bcadbd16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 23:06:01 +0200 Subject: [PATCH 083/136] Fix print load for non-NuttX configs --- src/modules/systemlib/printload.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 09f8a562b9..ee210f8903 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -43,8 +43,13 @@ __BEGIN_DECLS +#include #include +#ifndef CONFIG_MAX_TASKS +#define CONFIG_MAX_TASKS 64 +#endif + struct print_load_s { uint64_t total_user_time; From 0d44510f673ca78cfda26b7c537a710b146c6d77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:12:40 +0200 Subject: [PATCH 084/136] Examples: Fix copyright --- src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/examples/px4_mavlink_debug/px4_mavlink_debug.c | 5 +++-- src/examples/px4_simple_app/px4_simple_app.c | 5 +++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 6e99939d14..adaa502abb 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c index aa1eb5ed8f..c1dd284632 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_mavlink_debug.c * Debug application example for PX4 autopilot + * + * @author Example User */ #include diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index e22c59fa22..bc5d8697ed 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /** * @file px4_simple_app.c * Minimal application example for PX4 autopilot + * + * @author Example User */ #include From 5c5ba1a85fd377bb6a0f1c04fcd70d08fa1718f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:39:49 +0200 Subject: [PATCH 085/136] Fix path for examples --- makefiles/config_px4fmu-v2_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 0998ebf9f8..9e8dab6850 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -140,19 +140,19 @@ MODULES += examples/rover_steering_control # #MODULES += examples/math_demo # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky +# https://px4.io/dev/px4_simple_app #MODULES += examples/px4_simple_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon +# https://px4.io/dev/daemon #MODULES += examples/px4_daemon_app # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values +# https://px4.io/dev/debug_values #MODULES += examples/px4_mavlink_debug # Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +# https://px4.io/dev/example_fixedwing_control #MODULES += examples/fixedwing_control # Hardware test From dcf7b81f8321555a9e0c209cfaf59726dea154f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:48:48 +0200 Subject: [PATCH 086/136] Airspeed: fix code style --- src/drivers/airspeed/airspeed.cpp | 75 +++++++++++++++++++------------ 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index b1679f7c57..772e924259 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * 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 @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file ets_airspeed.cpp - * @author Simon Wilks + * @file airspeed.cpp + * @author Simon Wilks + * @author Lorenz Meier * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ @@ -76,7 +77,7 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -105,12 +106,14 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -124,13 +127,16 @@ Airspeed::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -146,8 +152,9 @@ Airspeed::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub < 0) + if (_airspeed_pub < 0) { warnx("uORB started?"); + } } ret = OK; @@ -166,7 +173,7 @@ Airspeed::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -179,20 +186,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -202,13 +209,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -217,15 +225,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -233,21 +243,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -261,16 +275,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +301,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +384,7 @@ Airspeed::update_status() if (_subsys_pub > 0) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,6 +418,7 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } From bac89be4b98869dce6899db2b18da4da52da471b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:48:58 +0200 Subject: [PATCH 087/136] Airspeed ETS: fix code style --- src/drivers/ets_airspeed/ets_airspeed.cpp | 77 +++++++++++++++-------- 1 file changed, 50 insertions(+), 27 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 4c75143aca..44d23e01de 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -94,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH); protected: @@ -113,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) +ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -155,15 +155,16 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - if (diff_pres_pa_raw == 0) { + + if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the // caller could end up using this value as part of an // average perf_count(_comms_errors); - log("zero value from sensor"); + log("zero value from sensor"); return -1; - } + } // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; @@ -175,7 +176,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); // XXX we may want to smooth out the readings to remove noise. report.differential_pressure_filtered_pa = diff_pres_pa_raw; @@ -210,6 +211,7 @@ ETSAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ @@ -239,6 +241,7 @@ ETSAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { debug("measure error"); } @@ -287,26 +290,31 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new ETSAirspeed(i2c_bus); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -351,21 +359,24 @@ test() int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -376,22 +387,25 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -404,14 +418,17 @@ reset() { int fd = open(ETS_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -422,8 +439,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { ets_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { ets_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ets_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { ets_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { ets_airspeed::info(); + } ets_airspeed_usage(); exit(0); From 135543f03f95f2860640d4145c01ebfe0480af47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 10:49:09 +0200 Subject: [PATCH 088/136] Airspeed MEAS: fix code style --- src/drivers/meas_airspeed/meas_airspeed.cpp | 27 ++++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 45684b5906..1db80769ca 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,7 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), + CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), _t_system_power(-1), system_power{} @@ -189,9 +189,11 @@ MEASAirspeed::collect() break; case 1: - /* fallthrough */ + + /* fallthrough */ case 2: - /* fallthrough */ + + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); @@ -219,11 +221,11 @@ MEASAirspeed::collect() are generated when the bottom port is used as the static port on the pitot and top port is used as the dynamic port */ - float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa_raw, temperature); + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa_raw, temperature); // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; @@ -276,6 +278,7 @@ MEASAirspeed::cycle() /* perform collection */ ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); @@ -304,6 +307,7 @@ MEASAirspeed::cycle() /* measurement phase */ ret = measure(); + if (OK != ret) { debug("measure error"); } @@ -332,18 +336,23 @@ void MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { _t_system_power = orb_subscribe(ORB_ID(system_power)); } + if (_t_system_power == -1) { // not available return; } + bool updated = false; orb_check(_t_system_power, &updated); + if (updated) { orb_copy(ORB_ID(system_power), _t_system_power, &system_power); } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { // not valid, skip correction return; @@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) apply a piecewise linear correction, flattening at 0.5V from 5V */ float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -0.5f) { voltage_diff = -0.5f; } + diff_press_pa -= voltage_diff * slope; /* @@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) */ const float temp_slope = 0.887f; voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { voltage_diff = 0.5f; } + if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From 6bb941218caf3d8aa70bcb180b219d85024b8a91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:05:09 +0200 Subject: [PATCH 089/136] Limit stack size of HIL app launcher --- src/drivers/hil/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f1fc49fb3d..17d42d0351 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -39,4 +39,6 @@ MODULE_COMMAND = hil SRCS = hil.cpp +MODULE_STACKSIZE = 1200 + MAXOPTIMIZATION = -Os From 4a839c7e7ea575b434d701c0db7c87afd7ce07c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:05:32 +0200 Subject: [PATCH 090/136] HIL: Do not start GPS deamon --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b21ca9ec88..1027d90eac 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -289,6 +289,7 @@ then if ver hwcmp PX4FMU_V1 then set FMU_MODE serial + set GPS no fi fi unset HIL @@ -319,6 +320,7 @@ then gps start fi fi + unset GPS unset GPS_FAKE # Needs to be this early for in-air-restarts From b235420f1700a5b4cddebd29d8aba441b1e340b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:06:04 +0200 Subject: [PATCH 091/136] HIL: Limit stack size of HIL app --- src/drivers/hil/hil.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 0fbabaf2f1..3cf6d2032a 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -231,7 +231,7 @@ HIL::init() _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 1000, (main_t)&HIL::task_main_trampoline, nullptr); From bfef24bf727f72e0f82050276777e000930d3c39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:13:55 +0200 Subject: [PATCH 092/136] Add airspeed boot workaround. From @kd0aij --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ed064e2366..39b4857ec9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -323,6 +323,8 @@ then unset GPS_FAKE # Needs to be this early for in-air-restarts + # Wait 10 ms for sensors (workaround for airspeed, to be removed) + usleep 10000 commander start # From 74b2d21e9474c07f408d3a00850dd486b3991eb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:33:04 +0200 Subject: [PATCH 093/136] HIL: Do not start GPS regardless of platform --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1027d90eac..3d36fe346c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -286,10 +286,10 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set GPS no if ver hwcmp PX4FMU_V1 then set FMU_MODE serial - set GPS no fi fi unset HIL From 3b7685a8fe3d251d3235f1b3d9c9289a41dd2bc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 11:34:38 +0200 Subject: [PATCH 094/136] PWM Sim: Fix merge fail --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 8aae4bcbfe..4bff7c1123 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -234,13 +234,8 @@ PWMSim::init() _task = px4_task_spawn_cmd("pwm_out_sim", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, -<<<<<<< HEAD:src/drivers/pwm_out_sim/pwm_out_sim.cpp - 1200, - (px4_main_t)&PWMSim::task_main_trampoline, -======= 1000, - (main_t)&HIL::task_main_trampoline, ->>>>>>> beta:src/drivers/hil/hil.cpp + (px4_main_t)&PWMSim::task_main_trampoline, nullptr); if (_task < 0) { From 4aec25724e92bf4b80780336fc2313d2aad3736c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 13:38:37 +0200 Subject: [PATCH 095/136] Condition the print load systemlib build correctly for all OS --- src/modules/systemlib/module.mk | 5 +++-- src/modules/systemlib/print_load_posix.c | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 194660776e..3232adf3e7 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -57,11 +57,12 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS += err.c \ printload.c \ up_cxxinitialize.c +else +SRCS += print_load_posix.c endif ifneq ($(PX4_TARGET_OS),qurt) -SRCS += hx_stream.c \ - print_load_posix.c +SRCS += hx_stream.c endif MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/print_load_posix.c b/src/modules/systemlib/print_load_posix.c index ef8571a7e8..5e12dc3323 100644 --- a/src/modules/systemlib/print_load_posix.c +++ b/src/modules/systemlib/print_load_posix.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file printload.c + * @file print_load_posix.c * * Print the current system load. * From b02af04f41f749fec236627a0ea9a7ce54cdebc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 14:02:20 +0200 Subject: [PATCH 096/136] Re-enable VTOL airframes on master --- ..._caipirinha_vtol => 13001_caipirinha_vtol} | 0 ...INACTIVE_13002_firefly6 => 13002_firefly6} | 0 ..._quad_tailsitter => 13003_quad_tailsitter} | 0 ...uad+_tailsitter => 13004_quad+_tailsitter} | 0 .../init.d/INACTIVE_13005_vtol_AAERT_quad | 50 ------------------- 5 files changed, 50 deletions(-) rename ROMFS/px4fmu_common/init.d/{INACTIVE_13001_caipirinha_vtol => 13001_caipirinha_vtol} (100%) rename ROMFS/px4fmu_common/init.d/{INACTIVE_13002_firefly6 => 13002_firefly6} (100%) rename ROMFS/px4fmu_common/init.d/{INACTIVE_13003_quad_tailsitter => 13003_quad_tailsitter} (100%) rename ROMFS/px4fmu_common/init.d/{INACTIVE_13004_quad+_tailsitter => 13004_quad+_tailsitter} (100%) delete mode 100644 ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad diff --git a/ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol similarity index 100% rename from ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol rename to ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol diff --git a/ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 similarity index 100% rename from ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6 rename to ROMFS/px4fmu_common/init.d/13002_firefly6 diff --git a/ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter rename to ROMFS/px4fmu_common/init.d/13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter similarity index 100% rename from ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter rename to ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad deleted file mode 100644 index 4b8191f557..0000000000 --- a/ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad +++ /dev/null @@ -1,50 +0,0 @@ -#!nsh -# -# @name Generic AAERT tailplane airframe with Quad VTOL. -# -# @type Standard VTOL -# -# @maintainer Simon Wilks -# - -sh /etc/init.d/rc.vtol_defaults - -if [ $AUTOCNF == yes ] -then - param set VT_TYPE 2 - param set VT_MOT_COUNT 4 - param set VT_TRANS_THR 0.75 - - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 - param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 - param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 - param set MC_YAW_FF 0.5 - param set MC_YAWRATE_P 0.22 - param set MC_YAWRATE_I 0.02 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_FF 0.0 -fi - -set MIXER vtol_quad_x -set PWM_OUT 12345678 - -set MIXER_AUX vtol_AAERT -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 - -set MAV_TYPE 22 - -param set VT_MOT_COUNT 4 -param set VT_IDLE_PWM_MC 1080 -param set VT_TYPE 2 From 47aedba691c8ca6ab42dda5600dcf42fb2746b85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:27:07 +0200 Subject: [PATCH 097/136] TECS: Fix altitude reset logic --- src/lib/external_lgpl/tecs/tecs.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 9e47590fd2..3c59add864 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -62,6 +62,10 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; + + // Reset the filter state as we just switched from non-altitude control + // to altitude control mode + _states_initalized = false; } _update_50hz_last_usec = now; From 3ccb35f338541c0f0aaf864e6e4f6181c9da19cc Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 23 Aug 2015 13:44:42 +0200 Subject: [PATCH 098/136] fix --- msg/tecs_status.msg | 4 ++-- src/lib/external_lgpl/tecs/tecs.cpp | 5 ++--- src/lib/external_lgpl/tecs/tecs.h | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 6 files changed, 13 insertions(+), 14 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 7e9d6670c9..4dcd16b2ca 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -23,7 +23,7 @@ float32 energyDistributionError float32 totalEnergyRateError float32 energyDistributionRateError -float32 throttle_sp -float32 pitch_sp +float32 throttle_integ +float32 pitch_integ uint8 mode diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 3c59add864..7607f71c16 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -620,9 +620,8 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.energy_error_integ = _integ6_state; _tecs_state.energy_distribution_error_integ = _integ7_state; - - _tecs_state.throttle_sp = _throttle_dem; - _tecs_state.pitch_sp = _pitch_dem; + _tecs_state.throttle_integ = _integ6_state; + _tecs_state.pitch_integ = _integ7_state; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 0fbda0fe9a..2599e7a152 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -155,8 +155,8 @@ public: float total_energy_rate_error; float energy_distribution_error; float energy_distribution_rate_error; - float throttle_sp; - float pitch_sp; + float throttle_integ; + float pitch_integ; enum ECL_TECS_MODE mode; }; 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 e14b84fb39..1666c4850e 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 @@ -1937,8 +1937,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.energyDistributionError = s.energy_distribution_error; t.energyDistributionRateError = s.energy_distribution_rate_error; - t.throttle_sp = s.throttle_sp; - t.pitch_sp = s.pitch_sp; + t.throttle_integ = s.throttle_integ; + t.pitch_integ = s.pitch_integ; if (_tecs_status_pub > 0) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ea843f9202..e00417797c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1886,8 +1886,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; - log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp; - log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp; + log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ; + log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1363b37ba7..95d4d96d5c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -368,8 +368,8 @@ struct log_TECS_s { float totalEnergyRateError; float energyDistributionError; float energyDistributionRateError; - float throttle_sp; - float pitch_sp; + float pitch_integ; + float throttle_integ; uint8_t mode; }; @@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), From 20d71870ec32f1538956ae13b07baf90b956a129 Mon Sep 17 00:00:00 2001 From: devbharat Date: Sun, 23 Aug 2015 12:20:26 +0200 Subject: [PATCH 099/136] Minor changes to get fixed wing control apps running for posix --- makefiles/posix/config_posix_sitl.mk | 8 ++++ src/lib/ecl/attitude_fw/ecl_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 24 +++++------ .../ecl/attitude_fw/ecl_roll_controller.cpp | 16 +++---- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 26 +++++------ src/lib/external_lgpl/tecs/tecs.cpp | 8 ++-- .../fw_att_control/fw_att_control_main.cpp | 43 ++++++++++--------- .../fw_pos_control_l1_main.cpp | 21 +++++---- 8 files changed, 81 insertions(+), 67 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 6b6708a5cc..57ede0b78b 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -49,6 +49,10 @@ MODULES += modules/mc_att_control MODULES += modules/mc_pos_control_multiplatform MODULES += modules/mc_att_control_multiplatform MODULES += modules/land_detector +MODULES += modules/fw_att_control +MODULES += modules/fw_pos_control_l1 + + # # Library modules @@ -67,9 +71,13 @@ MODULES += modules/controllib # MODULES += lib/mathlib MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += lib/launchdetection + # # POSIX port diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 95ccf4130f..7bf4129e48 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate() float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { float airspeed_result = airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b1304b34bf..d04fe38ac3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -62,10 +62,10 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch_setpoint) && - isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.airspeed))) { + if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && + PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; @@ -94,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 5935f15ceb..9d40c73f3b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController() float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { + if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.pitch) && - isfinite(ctl_data.roll_rate) && - isfinite(ctl_data.yaw_rate) && - isfinite(ctl_data.yaw_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && - isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.roll_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && + PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d1b6f1bfeb..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && - isfinite(ctl_data.pitch) && - isfinite(ctl_data.speed_body_u) && - isfinite(ctl_data.speed_body_v) && - isfinite(ctl_data.speed_body_w) && - isfinite(ctl_data.roll_rate_setpoint) && - isfinite(ctl_data.pitch_rate_setpoint))) { + if (!(PX4_ISFINITE(ctl_data.roll) && + PX4_ISFINITE(ctl_data.pitch) && + PX4_ISFINITE(ctl_data.speed_body_u) && + PX4_ISFINITE(ctl_data.speed_body_v) && + PX4_ISFINITE(ctl_data.speed_body_w) && + PX4_ISFINITE(ctl_data.roll_rate_setpoint) && + PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control // counter++; - if (!isfinite(_rate_setpoint)) { + if (!PX4_ISFINITE(_rate_setpoint)) { warnx("yaw rate sepoint not finite"); _rate_setpoint = 0.0f; } @@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && - isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && - isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && - isfinite(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl /* input conditioning */ float airspeed = ctl_data.airspeed; - if (!isfinite(airspeed)) { + if (!PX4_ISFINITE(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 7607f71c16..ec2d64ad83 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -98,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix< // Only required if airspeed is being measured and controlled float temp = 0; - if (isfinite(airspeed) && airspeed_sensor_enabled()) { + if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { // Get DCM // Calculate speed rate of change // XXX check @@ -142,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, // Get airspeed or default to halfway between min and max if // airspeed is not being used and set speed rate to zero - if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { // If no airspeed available use average of min and max _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); @@ -228,12 +228,12 @@ void TECS::_update_speed_demand(void) void TECS::_update_height_demand(float demand, float state) { // Handle initialization - if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { + if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) { _hgt_dem_in_old = demand; } // Apply 2 point moving average to demanded height // This is required because height demand is updated in steps - if (isfinite(demand)) { + if (PX4_ISFINITE(demand)) { _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); } else { _hgt_dem = _hgt_dem_in_old; 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 d073289952..78ee9812bf 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -42,6 +42,9 @@ */ #include +#include +#include +#include #include #include #include @@ -421,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -800,7 +803,7 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (isfinite(_manual.flaps)) { + if (PX4_ISFINITE(_manual.flaps)) { flaps_control = _manual.flaps; } @@ -810,7 +813,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; if (nonfinite) { @@ -990,7 +993,7 @@ FixedwingAttitudeControl::task_main() control_input.lock_integrator = lock_integrator; /* Run attitude controllers */ - if (isfinite(roll_sp) && isfinite(pitch_sp)) { + if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude @@ -1002,8 +1005,8 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; - if (!isfinite(roll_u)) { + _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1013,8 +1016,8 @@ FixedwingAttitudeControl::task_main() } float pitch_u = _pitch_ctrl.control_bodyrate(control_input); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; - if (!isfinite(pitch_u)) { + _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1034,11 +1037,11 @@ FixedwingAttitudeControl::task_main() } float yaw_u = _yaw_ctrl.control_bodyrate(control_input); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; - if (!isfinite(yaw_u)) { + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { @@ -1048,11 +1051,11 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ - _actuators.control[3] = (isfinite(throttle_sp) && + _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && !(_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd)) ? throttle_sp : 0.0f; - if (!isfinite(throttle_sp)) { + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } @@ -1145,7 +1148,7 @@ FixedwingAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingAttitudeControl::task_main_trampoline, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1159,23 +1162,23 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_att_control {start|stop|status}"); + warnx("usage: fw_att_control {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (att_control::g_control != nullptr) - errx(1, "already running"); + warnx("already running"); att_control::g_control = new FixedwingAttitudeControl; if (att_control::g_control == nullptr) - errx(1, "alloc failed"); + warnx("alloc failed"); if (OK != att_control::g_control->start()) { delete att_control::g_control; att_control::g_control = nullptr; - err(1, "start failed"); + warn("start failed"); } /* check if the waiting is necessary at all */ @@ -1194,7 +1197,7 @@ int fw_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (att_control::g_control == nullptr) - errx(1, "not running"); + warnx("not running"); delete att_control::g_control; att_control::g_control = nullptr; @@ -1203,10 +1206,10 @@ int fw_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (att_control::g_control) { - errx(0, "running"); + warnx("running"); } else { - errx(1, "not running"); + warnx("not running"); } } 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 82326e8c1d..68d5486fa5 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 @@ -52,6 +52,9 @@ */ #include +#include +#include +#include #include #include #include @@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (!isfinite(global_pos.terrain_alt)) { + if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; } @@ -1958,7 +1961,7 @@ FixedwingPositionControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&FixedwingPositionControl::task_main_trampoline, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1972,16 +1975,16 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + warnx("usage: fw_pos_control_l1 {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (l1_control::g_control != nullptr) - errx(1, "already running"); + warnx("already running"); if (OK != FixedwingPositionControl::start()) { - err(1, "start failed"); + warn("start failed"); } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1997,7 +2000,7 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (l1_control::g_control == nullptr) - errx(1, "not running"); + warnx("not running"); delete l1_control::g_control; l1_control::g_control = nullptr; @@ -2006,10 +2009,10 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { - errx(0, "running"); + warnx("running"); } else { - errx(1, "not running"); + warnx("not running"); } } From 8257f3d2fd27beed0f24494d5faa9a482a825c60 Mon Sep 17 00:00:00 2001 From: devbharat Date: Sun, 23 Aug 2015 13:13:38 +0200 Subject: [PATCH 100/136] Replaced exits with return --- .../fw_att_control/fw_att_control_main.cpp | 15 +++++++++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++---- 2 files changed, 21 insertions(+), 8 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 78ee9812bf..6f725b247e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1135,7 +1135,6 @@ FixedwingAttitudeControl::task_main() _control_task = -1; _task_running = false; - _exit(0); } int @@ -1163,22 +1162,26 @@ int fw_att_control_main(int argc, char *argv[]) { if (argc < 2) { warnx("usage: fw_att_control {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (att_control::g_control != nullptr) warnx("already running"); + return 1; att_control::g_control = new FixedwingAttitudeControl; if (att_control::g_control == nullptr) warnx("alloc failed"); + return 1; if (OK != att_control::g_control->start()) { delete att_control::g_control; att_control::g_control = nullptr; warn("start failed"); + return 1; } /* check if the waiting is necessary at all */ @@ -1192,24 +1195,28 @@ int fw_att_control_main(int argc, char *argv[]) } printf("\n"); } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) + if (att_control::g_control == nullptr){ warnx("not running"); + return 1; + } delete att_control::g_control; att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (att_control::g_control) { warnx("running"); + return 0; } else { warnx("not running"); + return 1; } } 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 68d5486fa5..cc715f54c1 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 @@ -1824,7 +1824,6 @@ FixedwingPositionControl::task_main() warnx("exiting.\n"); _control_task = -1; - _exit(0); } void FixedwingPositionControl::reset_takeoff_state() @@ -1976,15 +1975,18 @@ int fw_pos_control_l1_main(int argc, char *argv[]) { if (argc < 2) { warnx("usage: fw_pos_control_l1 {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (l1_control::g_control != nullptr) warnx("already running"); + return 1; if (OK != FixedwingPositionControl::start()) { warn("start failed"); + return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1995,24 +1997,28 @@ int fw_pos_control_l1_main(int argc, char *argv[]) } printf("\n"); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr) + if (l1_control::g_control == nullptr){ warnx("not running"); + return 1; + } delete l1_control::g_control; l1_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (l1_control::g_control) { warnx("running"); + return 0; } else { warnx("not running"); + return 1; } } From feb3cc92705092385957a738813dc334341fc56f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 14 Aug 2015 10:18:59 +0200 Subject: [PATCH 101/136] Default attitude mappings to zero --- src/modules/sensors/sensor_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd297a95ee..eba4d7077c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1887,7 +1887,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_ROLL, 0); /** * Pitch control channel mapping. @@ -1900,7 +1900,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 0); /** * Failsafe channel mapping. @@ -1927,7 +1927,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); /** * Yaw control channel mapping. @@ -1940,7 +1940,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_YAW, 4); +PARAM_DEFINE_INT32(RC_MAP_YAW, 0); /** * Mode switch channel mapping. From ef3374b0c2d728cdaa167ec2c233ac02d09253c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Aug 2015 11:57:39 +0200 Subject: [PATCH 102/136] ROMFS: Add transitional support for RC map default reset in --- ROMFS/px4fmu_common/init.d/rcS | 64 ++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index dfa3af4a79..2f474fa539 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -89,6 +89,70 @@ then fi fi + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_THROTTLE 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC3_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_THROTTLE 3 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_ROLL 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC1_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_ROLL 1 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_PITCH 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC2_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_ROLL 2 + fi + fi + + # Compare existing params and save defaults + # this only needs to be in for 1-2 releases + if param compare RC_MAP_YAW 0 + then + # So this is a default setup, + # now lets find out if channel 3 + # is calibrated + if param compare RC4_MIN 1000 + then + # This is default, do nothing + else + # Set old default + param set RC_MAP_ROLL 4 + fi + fi + # # Start system state indicator # From d26aa2c85b9d9232a38d1d1283296f40ba80d7dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 17:41:15 +0200 Subject: [PATCH 103/136] Fix navigator priority --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1d252ea646..d63f5fe338 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -524,7 +524,7 @@ Navigator::start() /* start the task */ _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX -5, + SCHED_PRIORITY_DEFAULT - 5, 1500, (px4_main_t)&Navigator::task_main_trampoline, nullptr); From a508330bdfb1d3cde17c3a4b79fc79027d71e526 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 17:41:45 +0200 Subject: [PATCH 104/136] Fix attitude controller braces --- src/modules/fw_att_control/fw_att_control_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 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 6f725b247e..b6265ffc06 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -667,8 +667,9 @@ FixedwingAttitudeControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1167,15 +1168,17 @@ int fw_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) + if (att_control::g_control != nullptr) { warnx("already running"); return 1; + } att_control::g_control = new FixedwingAttitudeControl; - if (att_control::g_control == nullptr) + if (att_control::g_control == nullptr) { warnx("alloc failed"); return 1; + } if (OK != att_control::g_control->start()) { delete att_control::g_control; From 86279dab6b28733caddae79826d4f8bfafbcc056 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 17:41:59 +0200 Subject: [PATCH 105/136] Fix position controller braces --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 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 cc715f54c1..486f9ac7f4 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 @@ -1731,8 +1731,9 @@ FixedwingPositionControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1980,9 +1981,10 @@ int fw_pos_control_l1_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (l1_control::g_control != nullptr) + if (l1_control::g_control != nullptr) { warnx("already running"); return 1; + } if (OK != FixedwingPositionControl::start()) { warn("start failed"); From f93e600b8c842ed84e91274db3e64076ab6b219a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 12 Aug 2015 16:12:44 +0200 Subject: [PATCH 106/136] consider transition state in posctl --- 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 bac72a81ce..7a67077a8a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2580,8 +2580,8 @@ set_control_mode() control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_position_enabled = !vtol_status.vtol_in_trans_mode; + control_mode.flag_control_velocity_enabled = !vtol_status.vtol_in_trans_mode; control_mode.flag_control_termination_enabled = false; break; From 5536360c49432420fb36eb85abae83804446d7eb Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 13 Aug 2015 09:00:15 +0200 Subject: [PATCH 107/136] use vehicle_status instead of vtol_status --- 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 7a67077a8a..74e94d9e3d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2580,8 +2580,8 @@ set_control_mode() control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !vtol_status.vtol_in_trans_mode; - control_mode.flag_control_velocity_enabled = !vtol_status.vtol_in_trans_mode; + control_mode.flag_control_position_enabled = !status.in_transition_mode; + control_mode.flag_control_velocity_enabled = !status.in_transition_mode; control_mode.flag_control_termination_enabled = false; break; @@ -2690,12 +2690,12 @@ set_control_mode() !offboard_control_mode.ignore_acceleration_force; control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode; + !offboard_control_mode.ignore_position) && !status.in_transition_mode; control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode; + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode; control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; From 22690ce897932748b9a2de1abe976a72bfaeaef1 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 19 Aug 2015 16:24:53 +0200 Subject: [PATCH 108/136] Take throttle from the appropriate controller given the current mode. --- src/modules/vtol_att_control/tiltrotor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ea9c688f2d..a50079b14b 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -342,10 +342,14 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; + } _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw _actuators_out_1->control[4] = _tilt_control; From 2c69d2d5d63b45a0bf863fb52037d0bdabc12232 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 19 Aug 2015 17:54:57 +0200 Subject: [PATCH 109/136] Make sure we allow multirotor attitude control for VTOLs when transitioning from FW to MC mode. --- src/modules/commander/commander.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74e94d9e3d..847c74f06e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -239,6 +239,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma void set_control_mode(); +bool stabilization_required(); + void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); @@ -2538,8 +2540,8 @@ set_control_mode() case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); - control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_rates_enabled = stabilization_required(); + control_mode.flag_control_attitude_enabled = stabilization_required(); control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; @@ -2707,6 +2709,15 @@ set_control_mode() } } +bool +stabilization_required() +{ + return (status.is_rotary_wing || // is a rotary wing, or + status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or + (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND + !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode +} + void print_reject_mode(struct vehicle_status_s *status_local, const char *msg) { From 8f5f3faf46242eb62ff9387429536ea5fcca13cc Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 20 Aug 2015 11:41:00 +0200 Subject: [PATCH 110/136] fw position controller should not publish att sp during a vtol transition --- 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 486f9ac7f4..e6ed69d455 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 @@ -1791,11 +1791,11 @@ FixedwingPositionControl::task_main() _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) { + if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); - } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) { + } else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } From b9e991a3df91ef26cc6417f2a1f4021ed4e30f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Aug 2015 10:13:21 +0200 Subject: [PATCH 111/136] UAVCAN fix compile warning on signedness --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 7a151d2cd2..5ef9de0d87 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -371,7 +371,7 @@ class VirtualCanDriver : public uavcan::ICanDriver, void waitFor(uavcan::MonotonicDuration duration) { - static const unsigned NsPerSec = 1000000000; + static const int NsPerSec = 1000000000; if (duration.isPositive()) { auto abstime = ::timespec(); From fa26928e5102aca73f0913d73730cfcff7d4b636 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 18:12:51 +0200 Subject: [PATCH 112/136] Save RAM on UART buffer size --- nuttx-configs/px4fmu-v2/nsh/defconfig | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e828e42be0..59abc22fb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -564,7 +564,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=1860 +CONFIG_USART2_TXBUFSIZE=1100 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -575,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=400 -CONFIG_USART3_TXBUFSIZE=400 +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -587,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=400 -CONFIG_UART4_TXBUFSIZE=400 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -599,8 +599,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=400 -CONFIG_USART6_TXBUFSIZE=400 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -611,8 +611,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=400 -CONFIG_UART7_TXBUFSIZE=400 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -623,8 +623,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=400 -CONFIG_UART8_TXBUFSIZE=400 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=5000 +CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From 27e41395405fa100d33f970d1046fbe1d50407a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Aug 2015 10:13:21 +0200 Subject: [PATCH 113/136] UAVCAN fix compile warning on signedness --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 7a151d2cd2..5ef9de0d87 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -371,7 +371,7 @@ class VirtualCanDriver : public uavcan::ICanDriver, void waitFor(uavcan::MonotonicDuration duration) { - static const unsigned NsPerSec = 1000000000; + static const int NsPerSec = 1000000000; if (duration.isPositive()) { auto abstime = ::timespec(); From f7e4a4852d3df293cbb98da1a0a377d39ca9c0d9 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 21 Aug 2015 16:51:11 +0200 Subject: [PATCH 114/136] Reset the altitude setpoint. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++--- 2 files changed, 11 insertions(+), 4 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 e6ed69d455..73c2bb07ac 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 @@ -1027,7 +1027,11 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) _althold_epv = _global_pos.epv; _was_in_deadband = true; } - + if (_vehicle_status.is_vtol) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { + _hold_alt = _global_pos.alt; + } + } return climbout_mode; } 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 84f7439a58..e4874ff75f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -993,9 +993,12 @@ MulticopterPositionControl::task_main() reset_yaw_sp = true; } - // XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition - if (_vehicle_status.in_transition_mode) { - reset_yaw_sp = true; + /* reset yaw and altitude setpoint for VTOL which are in fw mode */ + if (_vehicle_status.is_vtol) { + if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + reset_yaw_sp = true; + _reset_alt_sp = true; + } } //Update previous arming state From 9e1713eb9f81a28dea7c025a9eb1944d489afd07 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 21 Aug 2015 17:54:07 +0200 Subject: [PATCH 115/136] constantly reset altitude and yaw setpoint in mc pos control when not in rotary wing mode --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e4874ff75f..dc98db02f3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -995,7 +995,7 @@ MulticopterPositionControl::task_main() /* reset yaw and altitude setpoint for VTOL which are in fw mode */ if (_vehicle_status.is_vtol) { - if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + if (!_vehicle_status.is_rotary_wing) { reset_yaw_sp = true; _reset_alt_sp = true; } From 3a474347491236fcb32ac6b58fe7d0cf78564f3b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 25 Aug 2015 21:59:01 -0700 Subject: [PATCH 116/136] Fixes for qurt HIL build Workaround required Eigen downgrade to 3.2. Hexagon toolchain does not support C++11 features of newest version of Eigen. Running make qurt_fixup will downgrade and patch Eigen for qurt. Running make restore will revert the patch and do a git submodule update to restore the expected Eigen version. Added a "restore" target to undo qurt_fixup Before doing a qurt build run: make qurt_fixup That will downgrade Eigen to 3.2 and apply the require patch. To build another target after downgrading: make restore Them make the desired target (other than qurt). Fixed type used in orb_priority to be consistent with the code (int* was used in declaration but int32_t* used in code) Removed unused class member variable in sensors.cpp Added cmake fix for unit tests. The location of px4_log.c changed. Fixed the qurt drv_hrt.c implementation to use us instead of ms for time resolution Added px4_led.c to nuttx platform layer Use the posix version of px4_led.c for nuttx so we don't end up with duplicate files. It was moved out of common because it is not used by qurt. Changed PX4_DEBUG to PX4_WARN when checking for the error condition for store_poll_waiter in vdev.cpp Updated the px4_log.h file to make calls to the qurt_log functions. The qurt_log function is defined in the platforms/qurt layer. Added an option to control starting the commander module in HIL mode. Moved the flight specific drivers to the configuration file instead of adding them to the common tool chain file because HIL mode does not need them. Added the uorb Subscriber and Publisher classes Call PX4_ISFINITE macro instead of isfinite(). Added px4_led.c to nuttx platform layer Use the posix version of px4_led.c for nuttx so we don't end up with duplicate files. It was moved out of common because it is not used by qurt. Signed-off-by: Mark Charlebois --- Makefile | 9 +- Tools/check_submodules.sh | 31 +-- makefiles/qurt/config_qurt_adsp.mk | 6 + makefiles/qurt/config_qurt_hil.mk | 1 + makefiles/qurt/setup.sh | 48 +++++ makefiles/qurt/toolchain_hexagon.mk | 4 - readme.md | 0 src/drivers/device/vdev.cpp | 5 +- src/drivers/device/vdev.h | 14 +- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 2 +- src/modules/commander/commander.cpp | 18 +- .../position_estimator_inav_main.c | 6 +- src/modules/sensors/sensors.cpp | 18 +- src/modules/uORB/module.mk | 9 +- src/modules/uORB/uORB.h | 2 +- src/platforms/common/module.mk | 3 +- src/platforms/common/px4_log.c | 5 - src/platforms/nuttx/px4_layer/module.mk | 3 +- src/platforms/posix/px4_layer/module.mk | 1 + src/platforms/posix/px4_layer/px4_log.c | 5 + src/platforms/px4_log.h | 95 +++++++-- src/platforms/px4_workqueue.h | 4 + .../qurt/eagle_px4_firmware_patch.patch | 190 ++++++++++++++++++ src/platforms/qurt/include/qurt_log.h | 64 ++++++ src/platforms/qurt/px4_layer/commands_hil.c | 2 +- src/platforms/qurt/px4_layer/drv_hrt.c | 16 +- src/platforms/qurt/px4_layer/main.cpp | 8 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 4 +- src/systemcmds/mixer/mixer.cpp | 6 +- unittests/CMakeLists.txt | 2 +- 30 files changed, 484 insertions(+), 97 deletions(-) create mode 100755 makefiles/qurt/setup.sh create mode 100644 readme.md delete mode 100644 src/platforms/common/px4_log.c create mode 100644 src/platforms/posix/px4_layer/px4_log.c create mode 100644 src/platforms/qurt/eagle_px4_firmware_patch.patch create mode 100644 src/platforms/qurt/include/qurt_log.h diff --git a/Makefile b/Makefile index 4632699433..a0dc23f705 100644 --- a/Makefile +++ b/Makefile @@ -118,6 +118,13 @@ ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/qurt/firmware_qurt.mk endif +qurt_fixup: + makefiles/qurt/setup.sh $(PX4_BASE) + +restore: + cd src/lib/eigen && git checkout . + git submodule update + # # Versioning # @@ -155,7 +162,7 @@ size: .PHONY: checksubmodules checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh $(PX4_TARGET_OS)) .PHONY: updatesubmodules updatesubmodules: diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index c2c6e3d7bb..5fa0831438 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -73,20 +73,27 @@ fi if [ -d src/lib/eigen ] then - STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") - if [ -z "$STATUSRETVAL" ] + echo "ARG = $1" + if [ $1 = "qurt" ] then - echo "Checked Eigen submodule, correct version found" + # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 + STATUSRETVAL=$(true) else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "eigen sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 + STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked Eigen submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "eigen sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi fi else git submodule update --init --recursive diff --git a/makefiles/qurt/config_qurt_adsp.mk b/makefiles/qurt/config_qurt_adsp.mk index 75b1bc429f..6a1ce1d5f2 100644 --- a/makefiles/qurt/config_qurt_adsp.mk +++ b/makefiles/qurt/config_qurt_adsp.mk @@ -1,3 +1,9 @@ +#Added configuration specific flags here. + +# For Actual flight we need to link against the driver dynamic libraries +LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 +LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc + # # Makefile for the EAGLE QuRT *default* configuration # diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index ee0288d764..0536d7a4ad 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -57,6 +57,7 @@ MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion +MODULES += modules/controllib # # QuRT port diff --git a/makefiles/qurt/setup.sh b/makefiles/qurt/setup.sh new file mode 100755 index 0000000000..8ffb13cc4b --- /dev/null +++ b/makefiles/qurt/setup.sh @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# ########################################################################### + +# +# This file is required to modify the PX4 project so it can be build with +# the Hexagon toolchain. +# +# 1. Downgrade Eigen to 3.2 because the C++11 features of latest version +# are not supported +# 2. Patch Eigen because the Hexagon toolchain complex.h defines "I" +# + +cd $1 +cd src/lib/eigen/ +git checkout . +git checkout e7850ed81f9c469e02df496ef09ae32ec0379b71 +patch -p1 < ../../../makefiles/qurt/qurt_eigen.patch + diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 556c2e21dc..038c299904 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -246,10 +246,6 @@ LDFLAGS += -g -mv5 -mG0lib -G0 -fpic -shared \ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) -# driver dynamic libraries -LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 -LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc - # Compiler support library # LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) diff --git a/readme.md b/readme.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0ed4d39ada..507cebbf83 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) /* yes? post the notification */ if (fds->revents != 0) sem_post(fds->sem); + } else { + PX4_WARN("Store Poll Waiter error."); } } else { @@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device - +} // namespace device \ No newline at end of file diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 3fd28c82ca..c39857bbf0 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -324,6 +324,13 @@ public: static const char *devList(unsigned int *next); static const char *topicList(unsigned int *next); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + protected: int register_driver(const char *name, void *data); @@ -406,13 +413,6 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - bool _pub_blocked; /**< true if publishing should be blocked */ private: diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 4bff7c1123..f42d166535 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -406,7 +406,7 @@ PWMSim::task_main() ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ - if (_mixers != nullptr) { + if (_armed && _mixers != nullptr) { /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 847c74f06e..a3996bf9e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -877,6 +877,17 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_armed = false; + bool startup_in_hil = false; + if(argc>0) { + if ( strcmp(argv[0],"-hil")==0) { + startup_in_hil = true; + } else { + PX4_ERR("Argument %s not supported.",argv[0]); + PX4_ERR("COMMANDER NOT STARTED"); + thread_should_exit = true; + } + } + /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); @@ -958,7 +969,12 @@ int commander_thread_main(int argc, char *argv[]) status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - status.hil_state = vehicle_status_s::HIL_STATE_OFF; + + if(startup_in_hil) { + status.hil_state = vehicle_status_s::HIL_STATE_ON; + } else { + status.hil_state = vehicle_status_s::HIL_STATE_OFF; + } status.failsafe = false; /* neither manual nor offboard control commands have been received */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 05687d0943..892e50f3a1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,10 +116,10 @@ static inline int max(int val1, int val2) static void usage(const char *reason) { if (reason) { - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); } - fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n"); return; } @@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (isfinite(sensor.baro_alt_meter)) { + if (PX4_ISFINITE(sensor.baro_alt_meter)) { baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cd5db542d3..b9337dc231 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -239,7 +239,6 @@ private: int32_t _mag2_prio; /**< mag2 sensor priority */ int32_t _baro_prio; /**< baro0 sensor priority */ int32_t _baro1_prio; /**< baro1 sensor priority */ - int32_t _diff_pres_prio; /**< baro1 sensor priority */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -1526,6 +1525,12 @@ Sensors::parameter_update_poll(bool forced) /* run through all mag sensors */ for (unsigned s = 0; s < 3; s++) { + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); @@ -1536,12 +1541,6 @@ Sensors::parameter_update_poll(bool forced) continue; } - /* set a valid default rotation (same as board). - * if the mag is configured, this might be replaced - * in the section below. - */ - _mag_rotation[s] = _board_rotation; - bool config_ok = false; /* run through all stored calibrations */ @@ -2124,7 +2123,7 @@ Sensors::task_main() warnx("Sensor initialization failed"); _sensors_task = -1; if (_fd_adc >=0) { - close(_fd_adc); + px4_close(_fd_adc); _fd_adc = -1; } return; @@ -2169,9 +2168,6 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */ diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 6a71fc25f4..b483c00a80 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -56,14 +56,11 @@ ifeq ($(PX4_TARGET_OS),posix-arm) SRCS += uORBTest_UnitTest.cpp endif -ifneq ($(PX4_TARGET_OS),qurt) -SRCS += Publication.cpp \ - Subscription.cpp -endif - SRCS += objects_common.cpp \ uORBUtils.cpp \ uORB.cpp \ - uORBMain.cpp + uORBMain.cpp \ + Publication.cpp \ + Subscription.cpp MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 3ae0f56a30..7b40cff219 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -338,7 +338,7 @@ extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT; * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_priority(int handle, int *priority) __EXPORT; +extern int orb_priority(int handle, int32_t *priority) __EXPORT; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk index 472dc7dba6..0e5f463e69 100644 --- a/src/platforms/common/module.mk +++ b/src/platforms/common/module.mk @@ -2,6 +2,5 @@ # Common OS porting APIs # -SRCS = px4_getopt.c \ - px4_log.c +SRCS = px4_getopt.c diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c deleted file mode 100644 index a2c61ab297..0000000000 --- a/src/platforms/common/px4_log.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; - -__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; diff --git a/src/platforms/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk index 1738063a56..93b55e67ab 100644 --- a/src/platforms/nuttx/px4_layer/module.mk +++ b/src/platforms/nuttx/px4_layer/module.mk @@ -35,6 +35,7 @@ # NuttX porting layer # -SRCS = px4_nuttx_tasks.c +SRCS = px4_nuttx_tasks.c \ + ../../posix/px4_layer/px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index 7d7e75afee..846ce9469c 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -40,5 +40,6 @@ SRCS = \ px4_posix_tasks.cpp \ lib_crc32.c \ drv_hrt.c \ + px4_log.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c new file mode 100644 index 0000000000..238b324194 --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -0,0 +1,5 @@ +#include + +__EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; + +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index ab6e0f287c..d071893b0c 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -38,6 +38,24 @@ #pragma once +#define _PX4_LOG_LEVEL_ALWAYS 0 +#define _PX4_LOG_LEVEL_DEBUG 1 +#define _PX4_LOG_LEVEL_WARN 2 +#define _PX4_LOG_LEVEL_ERROR 3 +#define _PX4_LOG_LEVEL_PANIC 4 + +// Used to silence unused variable warning +static inline void do_nothing(int level, ...) +{ + (void)level; +} + +/**************************************************************************** + * __px4_log_omit: + * Compile out the message + ****************************************************************************/ +#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) + #if defined(__PX4_ROS) #include @@ -47,6 +65,54 @@ #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) #define PX4_DEBUG(...) +#elif defined(__PX4_QURT) +#include "qurt_log.h" +/**************************************************************************** + * Messages that should never be filtered or compiled out + ****************************************************************************/ +#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#if defined(TRACE_BUILD) +/**************************************************************************** + * Extremely Verbose settings for a Trace build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(DEBUG_BUILD) +/**************************************************************************** + * Verbose settings for a Debug build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_DEBUG, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) + +#elif defined(RELEASE_BUILD) +/**************************************************************************** + * Non-verbose settings for a Release build to minimize strings in build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#else +/**************************************************************************** + * Medium verbose settings for a default build + ****************************************************************************/ +#define PX4_PANIC(FMT, ...) qurt_log(_PX4_LOG_LEVEL_PANIC, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ERROR, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) qurt_log(_PX4_LOG_LEVEL_WARN, __FILENAME__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) + +#endif +#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILENAME__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) + #else #define __STDC_FORMAT_MACROS @@ -59,18 +125,6 @@ __BEGIN_DECLS __EXPORT extern uint64_t hrt_absolute_time(void); -// Used to silence unused variable warning -static inline void do_nothing(int level, ...) -{ - (void)level; -} - -#define _PX4_LOG_LEVEL_ALWAYS 0 -#define _PX4_LOG_LEVEL_PANIC 1 -#define _PX4_LOG_LEVEL_ERROR 2 -#define _PX4_LOG_LEVEL_WARN 3 -#define _PX4_LOG_LEVEL_DEBUG 4 - __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; @@ -107,11 +161,22 @@ __EXPORT extern int __px4_log_level_current; * Use these to implement the code level macros below ****************************************************************************/ + /**************************************************************************** - * __px4_log_omit: - * Compile out the message + * __px4_log_named_cond: + * Convert a message in the form: + * PX4_LOG_COND(__dbg_enabled, "val is %d", val); + * to + * printf("%-5s val is %d\n", "LOG", val); + * if the first arg/condition is true. ****************************************************************************/ -#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__) +#define __px4_log_named_cond(name, cond, FMT, ...) \ + __px4__log_startcond(cond)\ + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + __px4__log_endline /**************************************************************************** * __px4_log_named_cond: diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 79162e3f6b..76e1218ba5 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -47,6 +47,10 @@ #include #include +#ifdef __PX4_QURT + #include +#endif + __BEGIN_DECLS #define HPWORK 0 diff --git a/src/platforms/qurt/eagle_px4_firmware_patch.patch b/src/platforms/qurt/eagle_px4_firmware_patch.patch new file mode 100644 index 0000000000..41ac284b51 --- /dev/null +++ b/src/platforms/qurt/eagle_px4_firmware_patch.patch @@ -0,0 +1,190 @@ +diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h +index 4ba2f98..a7b664e 100644 +--- a/src/include/mavlink/mavlink_log.h ++++ b/src/include/mavlink/mavlink_log.h +@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink critical message and print to console. +@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__) ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); ++*/ + + /** + * Send a mavlink emergency message and print to console. +@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +-#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ +- fprintf(stderr, "telem> "); \ ++#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); ++/* fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); +- ++*/ + struct mavlink_logmessage { + char text[MAVLINK_LOG_MAXLEN + 1]; + unsigned char severity; +diff --git a/src/lib/eigen b/src/lib/eigen +--- a/src/lib/eigen ++++ b/src/lib/eigen +@@ -1 +1 @@ +-Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 ++Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty +diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp +index bbf5f8e..50c7d8b 100644 +--- a/src/modules/commander/PreflightCheck.cpp ++++ b/src/modules/commander/PreflightCheck.cpp +@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro + } + } + ++ ++#ifdef __PX4_QURT ++ // WARNING: Preflight checks are important and should be added back when ++ // all the sensors are supported ++ PX4_WARN("WARNING: Preflight checks PASS always."); ++ failed = false; ++#endif ++ + /* Report status */ + return !failed; + } +diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp +index 971e086..c165322 100644 +--- a/src/modules/commander/commander.cpp ++++ b/src/modules/commander/commander.cpp +@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[]) + } + + /* RC input check */ ++#ifndef __PX4_QURT + if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && +- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { ++#else ++ // HACK: remove old data check due to timestamp issue in QURT ++ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) { ++#endif + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; +@@ -2526,7 +2531,7 @@ set_control_mode() + /* set vehicle_control_mode according to set_navigation_state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); +- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; ++ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; + control_mode.flag_control_offboard_enabled = false; + + switch (status.nav_state) { +diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c +index 892e50f..7ac6776 100644 +--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c ++++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c +@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) + }; + + /* wait for initial baro value */ ++#ifdef __PX4_QURT ++ // WARNING skipping ++ PX4_WARN("Hacked to skip baro initialization for testing."); ++ bool wait_baro = false; ++#else + bool wait_baro = true; ++#endif + + thread_running = true; + +diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp +index 3fa4458..0ef1883 100644 +--- a/src/modules/sensors/sensors.cpp ++++ b/src/modules/sensors/sensors.cpp +@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + ++#ifdef __PX4_QURT ++// Temporary fix required due to lack of time sync between apps and dsp cores ++ raw.timestamp = hrt_absolute_time(); ++#else + raw.timestamp = gyro_report.timestamp; ++#endif + raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; + } +@@ -2068,6 +2073,7 @@ Sensors::task_main() + + /* start individual sensors */ + int ret = 0; ++#if 0 + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; +@@ -2091,7 +2097,7 @@ Sensors::task_main() + } + return; + } +- ++#endif + /* + * do subscriptions + */ +diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp +index 83a9378..ec86faf 100644 +--- a/src/systemcmds/mixer/mixer.cpp ++++ b/src/systemcmds/mixer/mixer.cpp +@@ -118,6 +118,8 @@ load(const char *devname, const char *fname) + return 1; + } + ++#ifndef __PX4_QURT ++ + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { + warnx("can't load mixer: %s", fname); + return 1; +@@ -125,6 +127,24 @@ load(const char *devname, const char *fname) + + /* XXX pass the buffer to the device */ + int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); ++#else ++ char newbuf[] = ++ "R: 4x 10000 10000 10000 0\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 4 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 5 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 6 10000 10000 0 -10000 10000\n" ++ "M: 1\n" ++ "O: 10000 10000 0 -10000 10000\n" ++ "S: 0 7 10000 10000 0 -10000 10000\n"; ++ /* XXX pass the buffer to the device */ ++ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); ++#endif + + if (ret < 0) { + warnx("error loading mixers from %s", fname); diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h new file mode 100644 index 0000000000..29ef6302e1 --- /dev/null +++ b/src/platforms/qurt/include/qurt_log.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef QURT_LOG_H +#define QURT_LOG_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//void qurt_log(int level, const char *file, int line, const char *format, ...); + +// declaration to make the compiler happy. This symbol is part of the adsp static image. +void HAP_debug(const char *msg, int level, const char *filename, int line); + +static __inline void qurt_log(int level, const char *file, int line, + const char *format, ...) +{ + char buf[256]; + va_list args; + va_start(args, format); + vsnprintf(buf, sizeof(buf), format, args); + va_end(args); + HAP_debug(buf, level, file, line); +} + +#ifdef __cplusplus +} +#endif + +#endif // QURT_LOG_H + diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index b795fba341..3f6c23b732 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -48,7 +48,7 @@ const char *get_commands() "param set CAL_MAG0_ID 196608\n" // "rgbled start\n" // "tone_alarm start\n" - "commander start\n" + "commander start -hil\n" "sensors start\n" //"ekf_att_pos_estimator start\n" "attitude_estimator_q start\n" diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 916992cae4..45be19ec8a 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -46,8 +46,6 @@ static struct sq_queue_s callout_queue; -extern uint64_t get_abs_time_in_us(); - /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; @@ -83,15 +81,9 @@ static void hrt_unlock(void) */ hrt_abstime hrt_absolute_time(void) { - - return get_abs_time_in_us(); -/* struct timespec ts; - - // FIXME - clock_gettime unsupported in QuRT - //clock_gettime(CLOCK_MONOTONIC, &ts); + clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); -*/ } /* @@ -252,7 +244,7 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; - uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); @@ -273,11 +265,11 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - ticks = USEC2TICK((next->deadline - now)*1000); + ticks = USEC2TICK((next->deadline - now)); } } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index da25cabcd2..f4a538a13a 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -165,11 +165,9 @@ int dspal_entry( int argc, char* argv[] ) px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for( ;; ) - { - volatile int x = 0; - ++x; - } + for( ;; ){ + usleep( 1000000 ); + } return 0; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index fcae64f8c5..9925e05dfa 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -59,9 +59,7 @@ __BEGIN_DECLS extern uint64_t get_ticks_per_us(); -// FIXME - sysconf(_SC_CLK_TCK) not supported -//long PX4_TICKS_PER_SEC = get_ticks_per_us(); -long PX4_TICKS_PER_SEC = 800000000; +long PX4_TICKS_PER_SEC = 1000; unsigned int sleep(unsigned int sec) { diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 8889f4531f..83a9378619 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -91,10 +91,10 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); - fprintf(stderr, "usage:\n"); - fprintf(stderr, " mixer load \n"); + PX4_INFO("usage:\n"); + PX4_INFO(" mixer load \n"); } static int diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 99a981f521..80eb8c24e6 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -75,7 +75,7 @@ endfunction() add_library( px4_platform # ${PX_SRC}/platforms/common/px4_getopt.c - ${PX_SRC}/platforms/common/px4_log.c + ${PX_SRC}/platforms/posix/px4_layer/px4_log.c ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp ${PX_SRC}/platforms/posix/work_queue/work_lock.c From 32ae3026be130c3fa353c1df9f74baa97993916f Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 25 Aug 2015 16:12:52 +0200 Subject: [PATCH 117/136] Mathlib: Fix to matrix division operator --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9f34bc81d5..a39cca335b 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -285,7 +285,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + res.data[i][j] = data[i][j] / num; return res; } From 3a3c4bbf74bc5e22042d14419f0c4db62247036e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 09:33:15 +0200 Subject: [PATCH 118/136] Remove bogus lower-caps readme file --- readme.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 readme.md diff --git a/readme.md b/readme.md deleted file mode 100644 index e69de29bb2..0000000000 From d00f78061fa4f9c7509df90e38735a383d5e9d72 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 25 Aug 2015 16:12:52 +0200 Subject: [PATCH 119/136] Mathlib: Fix to matrix division operator --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 2a7b61238b..ea23e8b7ca 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -294,7 +294,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + res.data[i][j] = data[i][j] / num; return res; } From 4238fe5b9c75f5f72526a441cfbd8ccbb2e92705 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 26 Aug 2015 11:45:25 +0200 Subject: [PATCH 120/136] set correct parameters for missing rc_map values --- ROMFS/px4fmu_common/init.d/rcS | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2f474fa539..85cbdeb59e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,7 +110,7 @@ then if param compare RC_MAP_ROLL 0 then # So this is a default setup, - # now lets find out if channel 3 + # now lets find out if channel 1 # is calibrated if param compare RC1_MIN 1000 then @@ -126,14 +126,14 @@ then if param compare RC_MAP_PITCH 0 then # So this is a default setup, - # now lets find out if channel 3 + # now lets find out if channel 2 # is calibrated if param compare RC2_MIN 1000 then # This is default, do nothing else # Set old default - param set RC_MAP_ROLL 2 + param set RC_MAP_PITCH 2 fi fi @@ -142,14 +142,14 @@ then if param compare RC_MAP_YAW 0 then # So this is a default setup, - # now lets find out if channel 3 + # now lets find out if channel 4 # is calibrated if param compare RC4_MIN 1000 then # This is default, do nothing else # Set old default - param set RC_MAP_ROLL 4 + param set RC_MAP_YAW 4 fi fi From 28d0ad48547a345a62df8d4cd36f533b3a27d819 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 26 Aug 2015 12:17:03 +0200 Subject: [PATCH 121/136] Don't try to convert gps time to seconds a second time round and if requested only use GPS time when a 3D fix is present. --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index aedbdff2da..c6d13f8756 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -201,7 +201,8 @@ static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; /* GPS time, used for log files naming */ -static uint64_t gps_time = 0; +static uint64_t gps_time_sec = 0; +static bool has_gps_3d_fix = false; /* current state of logging */ static bool logging_enabled = false; @@ -413,13 +414,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec; - if (_gpstime_only) { - utc_time_sec = gps_time / 1e6; + if (_gpstime_only && has_gps_3d_fix) { + utc_time_sec = gps_time_sec; } else { utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); } - if (utc_time_sec > PX4_EPOCH_SECS) { + if (utc_time_sec > PX4_EPOCH_SECS) { /* strip the time elapsed since boot */ if (boot_time) { utc_time_sec -= hrt_absolute_time() / 1e6; @@ -1004,7 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_usage(NULL); } - gps_time = 0; + gps_time_sec = 0; /* interpret logging params */ @@ -1304,7 +1305,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1335,7 +1336,8 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; + has_gps_3d_fix = buf_gps_pos.fix_type == 3; } if (!logging_enabled) { @@ -1992,7 +1994,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - warnx("time: gps: %u seconds", (unsigned)gps_time); + warnx("time: gps: %u seconds", (unsigned)gps_time_sec); if (!logging_enabled) { warnx("not logging"); } else { From 56adce9432f642b303cc8fa9e68ca7cfcebe0f1f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 26 Aug 2015 12:17:03 +0200 Subject: [PATCH 122/136] Don't try to convert gps time to seconds a second time round and if requested only use GPS time when a 3D fix is present. --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e00417797c..9c7554fa5d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -200,7 +200,8 @@ static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; /* GPS time, used for log files naming */ -static uint64_t gps_time = 0; +static uint64_t gps_time_sec = 0; +static bool has_gps_3d_fix = false; /* current state of logging */ static bool logging_enabled = false; @@ -412,13 +413,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec; - if (_gpstime_only) { - utc_time_sec = gps_time / 1e6; + if (_gpstime_only && has_gps_3d_fix) { + utc_time_sec = gps_time_sec; } else { utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); } - if (utc_time_sec > PX4_EPOCH_SECS) { + if (utc_time_sec > PX4_EPOCH_SECS) { /* strip the time elapsed since boot */ if (boot_time) { utc_time_sec -= hrt_absolute_time() / 1e6; @@ -987,7 +988,7 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_usage(NULL); } - gps_time = 0; + gps_time_sec = 0; /* interpret logging params */ @@ -1282,7 +1283,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1313,7 +1314,8 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec / 1e6; + gps_time_sec = buf_gps_pos.time_utc_usec / 1e6; + has_gps_3d_fix = buf_gps_pos.fix_type == 3; } if (!logging_enabled) { @@ -1955,7 +1957,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - warnx("time: gps: %u seconds", (unsigned)gps_time); + warnx("time: gps: %u seconds", (unsigned)gps_time_sec); if (!logging_enabled) { warnx("not logging"); } else { From 4143c9402972c28aba55a3c0376fb8d4dabf0d45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 14:15:32 +0200 Subject: [PATCH 123/136] sensors app: Re-introduce sensor rate limiting --- src/modules/sensors/sensors.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b9337dc231..26f7b1da30 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2168,6 +2168,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ From 7fb22e77f6d3a8bd26a6e12afde4c6e2f0d2eebc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 15:54:02 +0200 Subject: [PATCH 124/136] Remove old file --- .../commander/state_machine_helper_posix.cpp | 634 ------------------ 1 file changed, 634 deletions(-) delete mode 100644 src/modules/commander/state_machine_helper_posix.cpp diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp deleted file mode 100644 index 000cbf366a..0000000000 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ /dev/null @@ -1,634 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.cpp - * State machine helper functions implementations - * - * @author Thomas Gubler - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" -#include "PreflightCheck.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -// This array defines the arming state transitions. The rows are the new state, and the columns -// are the current state. Using new state and current state you can index into the array which -// will be true for a valid transition or false for a invalid transition. In some cases even -// though the transition is marked as true additional checks must be made. See arming_state_transition -// code for those checks. -static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI -}; - -// You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", -}; - -transition_result_t -arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status - const struct safety_s *safety, ///< current safety settings - arming_state_t new_arming_state, ///< arming state requested - struct actuator_armed_s *armed, ///< current armed status - bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing - const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none -{ - // Double check that our static arrays are still valid - ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); - ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); - - transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; - bool feedback_provided = false; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_arming_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - /* - * Get sensing state if necessary - */ - int prearm_ret = OK; - - /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - prearm_ret = prearm_check(status, mavlink_fd); - } - - /* - * Perform an atomic state update - */ - //irqstate_t flags = irqsave(); - - /* enforce lockdown in HIL */ - if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { - armed->lockdown = true; - - } else { - armed->lockdown = false; - } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - - // Do not perform pre-arm checks if coming from in air restore - // Allow if vehicle_status_s::HIL_STATE_ON - if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - - // Fail transition if pre-arm check fails - if (prearm_ret) { - /* the prearm check already prints the reject reason */ - feedback_provided = true; - valid_transition = false; - - // Fail transition if we need safety switch press - } else if (safety->safety_switch_available && !safety->safety_off) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); - feedback_provided = true; - valid_transition = false; - } - - // Perform power checks only if circuit breaker is not - // engaged for these checks - if (!status->circuit_breaker_engaged_power_check) { - // Fail transition if power is not good - if (!status->condition_power_input_valid) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); - feedback_provided = true; - valid_transition = false; - } - - // Fail transition if power levels on the avionics rail - // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { - // Check avionics rail voltages - if (status->avionics_power_rail_voltage < 4.75f) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; - } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - } - } - } - - } - - } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { - new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - warnx("HIL STATE ON -> STANDBY"); - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - warnx("NOT ARMING: Sensors not operational."); - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - - /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ - if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && - new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); - } else { - mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); - } - feedback_provided = true; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - } - - /* end of atomic state update */ - //irqrestore(flags); - } - - if (ret == TRANSITION_DENIED) { - const char * str = "INVAL: %s - %s"; - /* only print to console here by default as this is too technical to be useful during operation */ - warnx(str, state_names[status->arming_state], state_names[new_arming_state]); - - /* print to MAVLink if we didn't provide any feedback yet */ - if (!feedback_provided) { - mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); - } - } - - return ret; -} - -bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) -{ - // System is safe if: - // 1) Not armed - // 2) Armed, but in software lockdown (HIL) - // 3) Safety switch is present AND engaged -> actuators locked - if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { - return true; - - } else { - return false; - } -} - -transition_result_t -main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if the same state is requested because conditions may have changed */ - switch (new_main_state) { - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ACRO: - ret = TRANSITION_CHANGED; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ - /* TODO: add this for fixedwing as well */ - if (!status->is_rotary_wing || - (status->condition_local_altitude_valid || - status->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ - if (status->condition_local_position_valid || - status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* need global position estimate */ - if (status->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* need global position and home position */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - ret = TRANSITION_CHANGED; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - - /* need offboard signal */ - if (!status->offboard_control_signal_lost) { - ret = TRANSITION_CHANGED; - } - - break; - - case vehicle_status_s::MAIN_STATE_MAX: - default: - break; - } - if (ret == TRANSITION_CHANGED) { - if (status->main_state != new_main_state) { - status->main_state = new_main_state; - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - transition_result_t ret = TRANSITION_DENIED; - - if (current_status->hil_state == new_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - switch (new_state) { - case vehicle_status_s::HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - ret = TRANSITION_DENIED; - break; - - case vehicle_status_s::HIL_STATE_ON: - if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - const char *devname; - unsigned int handle = 0; - for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) - break; - - /* skip mavlink */ - if (!strcmp(MAVLINK_LOG_DEVICE, devname)) { - continue; - } - - - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); - ret = TRANSITION_DENIED; - } - break; - - default: - warnx("Unknown HIL state"); - break; - } - } - - if (ret == TRANSITION_CHANGED) { - current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - // XXX also set lockdown here - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - } - return ret; -} - -/** - * Check failsafe and main status and set navigation status for navigator accordingly - */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) -{ - navigation_state_t nav_state_old = status->nav_state; - - bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); - status->failsafe = false; - - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - case vehicle_status_s::MAIN_STATE_MANUAL: - case vehicle_status_s::MAIN_STATE_ALTCTL: - case vehicle_status_s::MAIN_STATE_POSCTL: - /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - } else { - switch (status->main_state) { - case vehicle_status_s::MAIN_STATE_ACRO: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; - break; - - case vehicle_status_s::MAIN_STATE_MANUAL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - - case vehicle_status_s::MAIN_STATE_ALTCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - break; - - case vehicle_status_s::MAIN_STATE_POSCTL: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - break; - - default: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - break; - } - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_MISSION: - - /* go into failsafe - * - if commanded to do so - * - if we have an engine failure - * - depending on datalink, RC and if the mission is finished */ - - /* first look at the commands */ - if (status->engine_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status->data_link_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->gps_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->rc_signal_lost_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - - /* finished handling commands which have priority, now handle failures */ - } else if (status->gps_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - - /* datalink loss enabled: - * check for datalink lost: this should always trigger RTGS */ - } else if (data_link_loss_enabled && status->data_link_lost) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* datalink loss disabled: - * check if both, RC and datalink are lost during the mission - * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ - } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || - (status->rc_signal_lost && mission_finished))) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ - } else if (!stay_in_failsafe){ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure */ - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* also go into failsafe if just datalink is lost */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* go into failsafe if RC is lost and datalink loss is not set up */ - } else if (status->rc_signal_lost && !data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - /* don't bother if RC is lost if datalink is connected */ - } else if (status->rc_signal_lost) { - - /* this mode is ok, we don't need RC for loitering */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } else { - /* everything is perfect */ - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - } - break; - - case vehicle_status_s::MAIN_STATE_AUTO_RTL: - /* require global position and home, also go into failsafe on an engine failure */ - - if (status->engine_failure) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((!status->condition_global_position_valid || - !status->condition_home_position_valid)) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - } - break; - - case vehicle_status_s::MAIN_STATE_OFFBOARD: - /* require offboard control, otherwise stay where you are */ - if (status->offboard_control_signal_lost && !status->rc_signal_lost) { - status->failsafe = true; - - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_local_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; - } - default: - break; - } - - return status->nav_state != nav_state_old; -} - -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) -{ - /* at this point this equals the preflight check, but might add additional - * quantities later. - */ - bool checkAirspeed = false; - /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - checkAirspeed = true; - } - - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); -} From 9ef9185d1dc0e43ecc9913b79196c2bd6d24d547 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 16:17:07 +0200 Subject: [PATCH 125/136] Commander: Fix commandline handling --- src/modules/commander/commander.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a3996bf9e2..397083a3b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -297,7 +297,7 @@ int commander_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 40, 3400, commander_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + (char * const *)&argv[0]); unsigned constexpr max_wait_us = 1000000; unsigned constexpr max_wait_steps = 2000; @@ -362,7 +362,7 @@ int commander_main(int argc, char *argv[]) if (calib_ret) { warnx("calibration failed, exiting."); - return 0; + return 1; } else { return 0; } @@ -391,7 +391,7 @@ int commander_main(int argc, char *argv[]) int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); px4_close(mavlink_fd_local); - return 0; + return 0; } usage("unrecognized command"); @@ -878,11 +878,17 @@ int commander_thread_main(int argc, char *argv[]) bool was_armed = false; bool startup_in_hil = false; - if(argc>0) { - if ( strcmp(argv[0],"-hil")==0) { + +#ifdef __PX4_NUTTX + /* NuttX indicates 3 arguments when only 2 are present */ + argc -= 1; +#endif + + if (argc > 2) { + if (!strcmp(argv[2],"-hil")) { startup_in_hil = true; } else { - PX4_ERR("Argument %s not supported.",argv[0]); + PX4_ERR("Argument %s not supported.", argv[2]); PX4_ERR("COMMANDER NOT STARTED"); thread_should_exit = true; } @@ -2238,7 +2244,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - //fflush(stdout); counter++; int blink_state = blink_msg_state(); From badb22bc23fed4c44387234d7e33b44022b14a31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 16:17:37 +0200 Subject: [PATCH 126/136] MAVLink app: Fix include guard to condition on NuttX when referring to NuttX --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7a6a207832..6fa5918fc5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1411,7 +1411,7 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; -#ifndef __PX4_POSIX +#ifdef __PX4_NUTTX /* the NuttX optarg handler does not * ignore argv[0] like the POSIX handler * does, nor does it deal with non-flag From 5db1490a03a820f44845bb68861cbae5920da81f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 20:07:56 +0200 Subject: [PATCH 127/136] EKF: Only publish with low EPH / EPV if the position estimate is actually valid / in place. --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 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 4588f3c901..078d06e092 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 @@ -932,9 +932,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; - if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + if (!_local_pos.xy_global || + !_local_pos.v_xy_valid || + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; + } else { _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; From ad1dff434d661dad7100e5eb7d2d7810d78a7c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 20:14:40 +0200 Subject: [PATCH 128/136] Navigator: Fix out of index warnings when empty offboard mission was read --- src/modules/navigator/mission.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b9e6e69439..350b05e39f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -624,26 +624,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s { /* select onboard/offboard mission */ int *mission_index_ptr; - struct mission_s *mission; dm_item_t dm_item; - int mission_index_next; + + struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; + int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + + /* do not work on empty missions */ + if (mission->count == 0) { + return false; + } + + /* move to next item if there is one */ + if (mission_index_next < ((int)mission->count - 1)) { + mission_index_next++; + } if (onboard) { /* onboard mission */ - mission_index_next = _current_onboard_mission_index + 1; mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; - - mission = &_onboard_mission; - dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_next = _current_offboard_mission_index + 1; mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; - - mission = &_offboard_mission; - dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -653,7 +656,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -665,7 +668,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_fd(), + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; } From 6927e62e7770e4943dfed2e4e74ad4ecf630ed10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Aug 2015 09:53:45 +0200 Subject: [PATCH 129/136] POSIX: Run pwm_out_sim as the hil command got renamed to it --- posix-configs/SITL/init/rc.fixed_wing | 2 +- posix-configs/SITL/init/rcS | 2 +- posix-configs/SITL/init/rc_iris_ros | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index a4a340cc38..65b69e4d0b 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -29,7 +29,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start fixedwing diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index f9518bfa02..6a9025bc97 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -34,7 +34,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start multicopter diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 9dc28a8505..558fa6464b 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -48,7 +48,7 @@ accelsim start barosim start adcsim start gpssim start -hil mode_pwm +pwm_out_sim mode_pwm commander start sensors start land_detector start multicopter From e30c822068cd17bda361b542126a2a267f7f8c0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Aug 2015 10:47:25 +0200 Subject: [PATCH 130/136] POSIX: Throw an error if the startup script is not present --- src/platforms/posix/main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index f4e398d186..9f914ed693 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -96,6 +96,11 @@ int main(int argc, char **argv) if (argc == 2) { ifstream infile(argv[1]); + if (!infile) { + cout << "failed opening script" << argv[1] << std::endl; + return 1; + } + for (string line; getline(infile, line, '\n'); ) { process_line(line); } From 17287ddd2c7d5829a5bfcbece7df2a4fe6f9ff80 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Aug 2015 10:48:38 +0200 Subject: [PATCH 131/136] POSIX: Fix start script path --- Tools/sitl_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 6e2cbc19be..3c65aa4eec 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -2,4 +2,4 @@ mkdir -p Build/posix_sitl.build/rootfs/fs/microsd mkdir -p Build/posix_sitl.build/rootfs/eeprom -cd Build/posix_sitl.build && ./mainapp ../$1 +cd Build/posix_sitl.build && ./mainapp ../../$1 From 68e726ce374d58d442799751088b99048445e237 Mon Sep 17 00:00:00 2001 From: mswingtra Date: Fri, 28 Aug 2015 11:06:12 +0200 Subject: [PATCH 132/136] px4_param_def to QGC fix --- Tools/px4params/srcparser.py | 16 +++++++++------- Tools/px4params/srcscanner.py | 10 ++++++++-- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 918040bbf5..c69699cad7 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -1,5 +1,7 @@ import sys import re +global default_var +default_var = {} class ParameterGroup(object): """ @@ -98,6 +100,7 @@ class SourceParser(object): re_comment_end = re.compile(r'(.*?)\s*\*\/') re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') re_cut_type_specifier = re.compile(r'[a-z]+$') re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') @@ -114,13 +117,6 @@ class SourceParser(object): def __init__(self): self.param_groups = {} - def GetSupportedExtensions(self): - """ - Returns list of supported file extensions that can be parsed by this - parser. - """ - return [".cpp", ".c"] - def Parse(self, contents): """ Incrementally parse program contents and append all found parameters @@ -193,6 +189,10 @@ class SourceParser(object): name = None defval = "" # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + if m: + name_m, defval_m = m.group(1,2) + default_var[name_m] = defval_m m = self.re_parameter_definition.match(line) if m: tp, name, defval = m.group(1, 2, 3) @@ -200,6 +200,8 @@ class SourceParser(object): m = self.re_px4_parameter_definition.match(line) if m: tp, name = m.group(1, 2) + if default_var.has_key(name+'_DEFAULT'): + defval = default_var[name+'_DEFAULT'] if tp is not None: # Remove trailing type specifier from numbers: 0.1f => 0.1 if defval != "" and self.re_is_a_number.match(defval): diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index a49039c719..4db5698992 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -13,10 +13,16 @@ class SourceScanner(object): Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = tuple(parser.GetSupportedExtensions()) + extensions1 = tuple([".h"]) + extensions2 = tuple([".cpp", ".c"]) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - if filename.endswith(extensions): + if filename.endswith(extensions1): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + for filename in filenames: + if filename.endswith(extensions2): path = os.path.join(dirname, filename) if not self.ScanFile(path, parser): return False From 37249470467e929ad618dec49f508dcd917f0b86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Aug 2015 12:18:25 +0200 Subject: [PATCH 133/136] Fix circuit breaker param typo --- src/modules/systemlib/circuit_breaker_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 097903d21f..054a1c3501 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety * - * Setting this parameter to 894281 will disable IO safety. + * Setting this parameter to 22027 will disable IO safety. * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 From 64248f3ad513d5384931532b848f2410a734af98 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 27 Aug 2015 18:45:40 +0200 Subject: [PATCH 134/136] mathlib: Bugfix to print function --- src/lib/mathlib/math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index a39cca335b..b81f8868ec 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -375,7 +375,7 @@ public: printf("[ "); for (unsigned int j = 0; j < N; j++) - printf("%.3f\t", data[i][j]); + printf("%.3f\t", (double)data[i][j]); printf(" ]\n"); } From aa1d7d78695664f69787228ca406b2fcc77f2878 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 28 Aug 2015 20:49:50 +0900 Subject: [PATCH 135/136] IRLock: fix compile error --- src/drivers/irlock/irlock.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 4aeb8c7497..19b0de6d5f 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -110,7 +110,7 @@ private: int read_device_block(struct irlock_s *block); /** internal variables **/ - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; work_s _work; uint32_t _read_failures; @@ -158,7 +158,7 @@ int IRLOCK::init() } /** allocate buffer storing values read from sensor **/ - _reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); + _reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s)); if (_reports == nullptr) { return ENOTTY; From 4eaa18e6f99f130c0c39f133c95997cd88458409 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Aug 2015 14:55:29 +1000 Subject: [PATCH 136/136] perf_counter: fixed write to correct fd for perf output this fixes perf output for nsh over MAVLink --- src/modules/systemlib/perf_counter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 1344accb84..b0a0b01d03 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle) void perf_print_counter(perf_counter_t handle) { - perf_print_counter_fd(0, handle); + perf_print_counter_fd(1, handle); } void