From a3b863bdd99bf556847ec64a88ddceb3071a897e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 13:50:59 +0200 Subject: [PATCH 01/42] UAVCAN: Receive commands from the GCS --- msg/vehicle_command.msg | 1 + src/modules/commander/commander.cpp | 1 + src/modules/uavcan/uavcan_servers.cpp | 29 ++++++++++++++++++++++++--- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 6ac66d22c7..3a4fb3a596 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -53,6 +53,7 @@ uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-b uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e86b7cfb9c..79d0811b72 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 451c5bbd81..2abadcafa4 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -53,9 +53,11 @@ #include "uavcan_servers.hpp" #include "uavcan_virtual_can_driver.hpp" -# include -# include -# include +#include +#include +#include + +#include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined @@ -277,6 +279,10 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) Lock lock(_subnode_mutex); + /* the subscribe call needs to happen in the same thread, + * so not in the constructor */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + while (1) { if (_check_fw == true) { @@ -286,6 +292,23 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + /* check if new commands are pending */ + bool cmd_ready; + orb_check(cmd_sub, &cmd_ready); + + if (cmd_ready) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { + warnx("received UAVCAN CONFIG command"); + + if (static_cast(cmd.param1 + 0.5f) == 1) { + warnx("UAVCAN CONFIG: Actuator assignment requested"); + } + } + } + if (spin_res < 0) { warnx("node spin error %i", spin_res); } From 250c91270436ff00dba9006cf23a9fdcdb57e5fd Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Tue, 6 Oct 2015 17:52:11 +1100 Subject: [PATCH 02/42] Added MAVLink/UAVCAN parameter bridge; implemented UAVCAN ESC enumeration --- msg/uavcan_parameter_request.msg | 8 + msg/uavcan_parameter_value.msg | 8 + src/modules/mavlink/mavlink_parameters.cpp | 145 ++++-- src/modules/mavlink/mavlink_parameters.h | 3 + src/modules/mavlink/mavlink_tests/module.mk | 53 --- src/modules/uavcan/uavcan_servers.cpp | 487 +++++++++++++++++++- src/modules/uavcan/uavcan_servers.hpp | 86 +++- 7 files changed, 689 insertions(+), 101 deletions(-) create mode 100644 msg/uavcan_parameter_request.msg create mode 100644 msg/uavcan_parameter_value.msg delete mode 100644 src/modules/mavlink/mavlink_tests/module.mk diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg new file mode 100644 index 0000000000..9ced52bbae --- /dev/null +++ b/msg/uavcan_parameter_request.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge request type +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/uavcan_parameter_value.msg b/msg/uavcan_parameter_value.msg new file mode 100644 index 0000000000..091c5fd278 --- /dev/null +++ b/msg/uavcan_parameter_value.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge response type +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 414b7ee96f..bdaa483dfc 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -41,25 +41,25 @@ #include +#include +#include + #include "mavlink_parameters.h" #include "mavlink_main.h" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), - _rc_param_map() + _rc_param_map(), + _uavcan_parameter_request_pub(nullptr), + _uavcan_parameter_value_sub(-1) { } unsigned MavlinkParametersManager::get_size() { - if (_send_all_index >= 0) { - return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - } else { - return 0; - } + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } void @@ -76,36 +76,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _send_all_index = 0; } + + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_list.target_component; + req.param_index = 0; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(msg, &set); + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); - if (set.target_system == mavlink_system.sysid && - (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find_no_notification(name); + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find_no_notification(name); - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown param: %s", name); - _mavlink->send_statustext_info(buf); + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); - } else { - /* set and send parameter */ - param_set(param, &(set.param_value)); - send_param(param); - } + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && + (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = set.target_component; + req.param_index = -1; + strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + if (set.param_type == MAV_PARAM_TYPE_REAL32) { + req.param_type = MAV_PARAM_TYPE_REAL32; + req.real_value = set.param_value; + } else { + int32_t val; + memcpy(&val, &set.param_value, sizeof(int32_t)); + req.param_type = MAV_PARAM_TYPE_INT64; + req.int_value = val; + } + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; @@ -144,6 +183,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } } } + + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && + (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_read.target_component; + req.param_index = req_read.param_index; + strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } @@ -192,11 +248,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && _mavlink->boot_complete()) { + bool space_available = _mavlink->get_free_tx_buf() >= get_size(); + + /* Send parameter values received from the UAVCAN topic */ + if (_uavcan_parameter_value_sub < 0) { + _uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value)); + } + + bool param_value_ready; + orb_check(_uavcan_parameter_value_sub, ¶m_value_ready); + if (space_available && param_value_ready) { + struct uavcan_parameter_value_s value; + orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); + + mavlink_param_value_t msg; + msg.param_count = value.param_count; + msg.param_index = value.param_index; + strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + if (value.param_type == MAV_PARAM_TYPE_REAL32) { + msg.param_type = MAVLINK_TYPE_FLOAT; + msg.param_value = value.real_value; + } else { + int32_t val; + val = (int32_t)value.int_value; + memcpy(&msg.param_value, &val, sizeof(int32_t)); + msg.param_type = MAVLINK_TYPE_INT32_T; + } + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); + } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { + /* send all parameters if requested, but only after the system has booted */ /* skip if no space is available */ - if (_mavlink->get_free_tx_buf() < get_size()) { + if (!space_available) { return; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 3dfed084b3..d258f3b240 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -118,4 +118,7 @@ protected: orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; + + orb_advert_t _uavcan_parameter_request_pub; + int _uavcan_parameter_value_sub; }; diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk deleted file mode 100644 index e104860937..0000000000 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ /dev/null @@ -1,53 +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. -# -############################################################################ - -# -# System state machine tests. -# - -MODULE_COMMAND = mavlink_tests -SRCS = mavlink_tests.cpp \ - mavlink_ftp_test.cpp \ - ../mavlink_stream.cpp \ - ../mavlink_ftp.cpp \ - ../mavlink.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink - -MODULE_STACKSIZE = 5000 - -MAXOPTIMIZATION = -Os - -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed - -EXTRACFLAGS = -Wno-packed diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 2abadcafa4..0dbf81153f 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -58,11 +58,20 @@ #include #include +#include +#include + +#include + +ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); +ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 + + /** * @file uavcan_servers.cpp * @@ -89,8 +98,25 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) : _node_info_retriever(_subnode), _fw_upgrade_trigger(_subnode, _fw_version_checker), _fw_server(_subnode, _fileserver_backend), + _count_in_progress(false), + _count_index(0), + _param_in_progress(0), + _param_index(0), + _param_list_in_progress(false), + _param_list_all_nodes(false), + _param_list_node_id(1), + _cmd_in_progress(false), + _param_response_pub(nullptr), + _param_getset_client(_subnode), _mutex_inited(false), - _check_fw(false) + _check_fw(false), + _esc_enumeration_active(false), + _esc_enumeration_index(0), + _beep_pub(_subnode), + _enumeration_indication_sub(_subnode), + _enumeration_client(_subnode), + _enumeration_getset_client(_subnode), + _enumeration_save_client(_subnode) { } @@ -272,7 +298,6 @@ int UavcanServers::init() return OK; } -__attribute__((optimize("-O0"))) pthread_addr_t UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); @@ -282,6 +307,27 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) /* the subscribe call needs to happen in the same thread, * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); + + /* Set up shared service clients */ + _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); + _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); + _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); + _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); + _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); + + uavcan::ServiceClient restartnode_client(_subnode); + restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); + + uavcan::ServiceClient opcode_client(_subnode); + opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); + + _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; + memset(_param_counts, 0, sizeof(_param_counts)); + + _esc_enumeration_active = false; + memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); + _esc_enumeration_index = 0; while (1) { @@ -290,21 +336,200 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _node_info_retriever.invalidateAll(); } - const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); - /* check if new commands are pending */ + // Check for parameter requests (get/set/list) + bool param_request_ready; + orb_check(param_request_sub, ¶m_request_ready); + + if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + struct uavcan_parameter_request_s request; + orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); + + if (_param_counts[request.node_id]) { + /* + * We know how many parameters are exposed by this node, so + * process the request. + */ + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { + uavcan::protocol::param::GetSet::Request req; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } else { + _param_in_progress = true; + _param_index = request.param_index; + warnx("UAVCAN command bridge: sent GetSet"); + } + } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { + uavcan::protocol::param::GetSet::Request req; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + if (request.param_type == MAV_PARAM_TYPE_REAL32) { + req.value.to() = request.real_value; + } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { + req.value.to() = request.int_value; + } else { + req.value.to() = request.int_value; + } + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } else { + _param_in_progress = true; + _param_index = request.param_index; + warnx("UAVCAN command bridge: sent GetSet"); + } + } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + // This triggers the _param_list_in_progress case below. + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = request.node_id; + _param_list_all_nodes = false; + + warnx("UAVCAN command bridge: starting component-specific param list"); + } + } else if (request.node_id == MAV_COMP_ID_ALL) { + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + /* + * This triggers the _param_list_in_progress case below, + * but additionally iterates over all active nodes. + */ + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = get_next_active_node_id(1); + _param_list_all_nodes = true; + + warnx("UAVCAN command bridge: starting global param list"); + + if (_param_counts[_param_list_node_id.get()] == 0) { + param_count(_param_list_node_id); + } + } + } else { + /* + * Need to know how many parameters this node has before we can + * continue; count them now and then process the request. + */ + param_count(request.node_id); + } + } + + // Handle parameter listing index/node ID advancement + if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + if (_param_index >= _param_counts[_param_list_node_id.get()]) { + // Reached the end of the current node's parameter set. + _param_list_in_progress = false; + + if (_param_list_all_nodes) { + // We're listing all parameters for all nodes -- get the next node ID + uavcan::NodeID next_id = get_next_active_node_id(_param_list_node_id); + if (next_id != _param_list_node_id) { + /* + * If there is a next node ID, check if that node's parameters + * have been counted before. If not, do it now. + */ + if (_param_counts[_param_list_node_id.get()] == 0) { + param_count(_param_list_node_id); + } + // Keep on listing. + _param_index = 0; + _param_list_in_progress = true; + warnx("UAVCAN command bridge: incrementing global param list node ID"); + } + } + } + } + + // Check if we're still listing, and need to get the next parameter + if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + // Ready to request the next value -- _param_index is incremented + // after each successful fetch by cb_getset + uavcan::protocol::param::GetSet::Request req; + req.index = _param_index; + + int call_res = _param_getset_client.call(_param_list_node_id, req); + if (call_res < 0) { + _param_list_in_progress = false; + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } else { + _param_in_progress = true; + warnx("UAVCAN command bridge: sent GetSet during param list operation"); + } + } + + // Check for ESC enumeration commands bool cmd_ready; orb_check(cmd_sub, &cmd_ready); - if (cmd_ready) { + if (cmd_ready && !_cmd_in_progress) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { - warnx("received UAVCAN CONFIG command"); + int command_id = static_cast(cmd.param1 + 0.5f); + int node_id = static_cast(cmd.param2 + 0.5f); + int call_res; - if (static_cast(cmd.param1 + 0.5f) == 1) { - warnx("UAVCAN CONFIG: Actuator assignment requested"); + warnx("UAVCAN command bridge: received command ID %d, node ID %d", command_id, node_id); + + switch (command_id) { + case 0: + case 1: { + _esc_enumeration_active = command_id; + _esc_enumeration_index = 0; + _esc_count = 0; + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); + } + break; + } + case 2: { + // Command is a restart node request + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + call_res = restartnode_client.call(node_id, restart_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent RestartNode"); + } + break; + } + case 3: + case 4: { + // Command is a param save or erase request + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = command_id == 3 ? opcode_req.OPCODE_SAVE : opcode_req.OPCODE_ERASE; + call_res = opcode_client.call(node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent ExecuteOpcode"); + } + break; + } + default: { + warnx("UAVCAN command bridge: unknown command ID %d", command_id); + break; + } } } } @@ -318,3 +543,249 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) return (pthread_addr_t) 0; } +void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uavcan::protocol::RestartNode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; +} + +void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; +} + +void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) +{ + if (_count_in_progress) { + /* + * Currently in parameter count mode: + * Iterate over all parameters for the node to which the request was + * originally sent, in order to find the maximum parameter ID. If a + * request fails, set the node's parameter count to zero. + */ + uint8_t node_id = result.getCallID().server_node_id.get(); + + if (result.isSuccessful()) { + warnx("UAVCAN command bridge: successful GetSet response during param count"); + + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + if (resp.name.size()) { + _param_counts[node_id] = _count_index++; + + uavcan::protocol::param::GetSet::Request req; + req.index = _count_index; + + int call_res = _param_getset_client.call(result.getCallID().server_node_id, req); + if (call_res < 0) { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res); + } else { + warnx("UAVCAN command bridge: sent GetSet during param count"); + } + } else { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]); + } + } else { + _param_counts[node_id] = 0; + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: GetSet error during param count"); + } + } else { + /* + * Currently in parameter get/set mode: + * Publish a uORB uavcan_parameter_value message containing the current value + * of the parameter. + */ + if (result.isSuccessful()) { + uavcan::protocol::param::GetSet::Response param = result.getResponse(); + + struct uavcan_parameter_value_s response; + response.node_id = result.getCallID().server_node_id.get(); + strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1); + response.param_id[16] = '\0'; + response.param_index = _param_index; + response.param_count = _param_counts[response.node_id]; + + if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + response.param_type = MAV_PARAM_TYPE_INT64; + response.int_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + response.param_type = MAV_PARAM_TYPE_REAL32; + response.real_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + response.param_type = MAV_PARAM_TYPE_UINT8; + response.int_value = param.value.to(); + } + + warnx("UAVCAN command bridge: successful GetSet response for param %s, node %hhu", response.param_id, response.node_id); + + if (_param_response_pub == nullptr) { + _param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response); + } else { + orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response); + } + } else { + warnx("UAVCAN command bridge: GetSet error"); + } + + _param_in_progress = false; + _param_index++; + } +} + +void UavcanServers::param_count(uavcan::NodeID node_id) +{ + uavcan::protocol::param::GetSet::Request req; + req.index = 0; + int call_res = _param_getset_client.call(node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res); + } else { + _count_in_progress = true; + _count_index = 0; + warnx("UAVCAN command bridge: starting param count"); + } +} + +uavcan::NodeID UavcanServers::get_next_active_node_id(const uavcan::NodeID &base) +{ + for (int i = base.get(); i < 128; i++) { + if (_node_info_retriever.isNodeKnown(i) && _subnode.getNodeID() != i) { + return uavcan::NodeID(i); + } + } + return base; +} + +void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) +{ + uavcan::NodeID next_id = get_next_active_node_id(result.getCallID().server_node_id); + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else if (result.getResponse().error) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error); + } else { + _esc_count++; + warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + } + + if (next_id != result.getCallID().server_node_id) { + // Still other active nodes to send the request to + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + + int call_res = _enumeration_client.call(next_id, req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request"); + } + } else { + warnx("UAVCAN ESC enumeration: completed enumeration on all nodes."); + } +} + +void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg) +{ + // Called whenever an ESC thinks it has received user input. + warnx("UAVCAN ESC enumeration: got indication"); + + if (!_esc_enumeration_active) { + // Ignore any messages received when we're not expecting them + return; + } + + // First, check if we've already seen an indication from this ESC. If so, + // just re-issue the previous get/set request. + int i; + for (i = 0; i < _esc_enumeration_index; i++) { + if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) { + warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d", _esc_enumeration_ids[i], i); + break; + } + } + + uavcan::protocol::param::GetSet::Request req; + req.name = "esc_index"; + req.value.to() = i; + + int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i); + } +} + +void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) +{ + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + uint8_t esc_index = (uint8_t)resp.value.to(); + esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index); + _esc_enumeration_index = std::max(_esc_enumeration_index, esc_index); + + _esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get(); + + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = opcode_req.OPCODE_SAVE; + int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index); + } + } +} + +void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult &result) +{ + uavcan::equipment::indication::BeepCommand beep; + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else if (!result.getResponse().ok) { + warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + beep.frequency = 440.0f; + beep.duration = 0.25f; + } + + (void)_beep_pub.broadcast(beep); + + if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 || + _esc_enumeration_index == _esc_count - 1) { + _esc_enumeration_active = false; + + // Tell all ESCs to stop enumerating + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = 0; + int call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration"); + } + } +} diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9d21b8f93f..78de54ccaf 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -42,16 +42,20 @@ #include #include -# include -# include -# include -# include -# include -# include -# include -# include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -# include "uavcan_virtual_can_driver.hpp" +#include "uavcan_virtual_can_driver.hpp" /** * @file uavcan_servers.hpp @@ -140,7 +144,71 @@ private: uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::BasicFileServer _fw_server; + /* + * The MAVLink parameter bridge needs to know the maximum parameter index + * of each node so that clients can determine when parameter listings have + * finished. We do that by querying a node's entire parameter set whenever + * a parameter message is received for a node with a zero _param_count, + * and storing the count here. If a node doesn't respond, or doesn't have + * any parameters, its count will stay at zero and we'll try to query the + * set again next time. + * + * The node's UAVCAN ID is used as the index into the _param_counts array. + */ + uint16_t _param_counts[128]; + bool _count_in_progress; + uint16_t _count_index; + + bool _param_in_progress; + uint16_t _param_index; + bool _param_list_in_progress; + bool _param_list_all_nodes; + uavcan::NodeID _param_list_node_id; + + bool _cmd_in_progress; + + // uORB topic handle for MAVLink parameter responses + orb_advert_t _param_response_pub; + + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_getset(const uavcan::ServiceCallResult &result); + void cb_count(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + + uavcan::ServiceClient _param_getset_client; + void param_count(uavcan::NodeID node_id); + + uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base); + bool _mutex_inited; volatile bool _check_fw; + // ESC enumeration + bool _esc_enumeration_active; + uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize]; + uint8_t _esc_enumeration_index; + uint8_t _esc_set_index; + uint8_t _esc_count; + + typedef uavcan::MethodBinder &)> EnumerationBeginCallback; + typedef uavcan::MethodBinder&)> + EnumerationIndicationCallback; + void cb_enumeration_begin(const uavcan::ServiceCallResult &result); + void cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg); + void cb_enumeration_getset(const uavcan::ServiceCallResult &result); + void cb_enumeration_save(const uavcan::ServiceCallResult &result); + + uavcan::Publisher _beep_pub; + uavcan::Subscriber _enumeration_indication_sub; + uavcan::ServiceClient _enumeration_client; + uavcan::ServiceClient _enumeration_getset_client; + uavcan::ServiceClient _enumeration_save_client; }; From f200260618438723ad4ccb783c54d7a58d1972d5 Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Sun, 11 Oct 2015 21:32:19 +1100 Subject: [PATCH 03/42] Exit uavcan fw process when armed --- src/modules/uavcan/uavcan_servers.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 0dbf81153f..e9eb573d36 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -308,6 +308,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); @@ -337,6 +338,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } // Check for parameter requests (get/set/list) bool param_request_ready; @@ -534,8 +538,18 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } } - if (spin_res < 0) { - warnx("node spin error %i", spin_res); + // Shut down once armed + // TODO (elsewhere): start up again once disarmed? + bool updated; + orb_check(armed_sub, &updated); + if (updated) { + struct actuator_armed_s armed; + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + if (armed.armed && !armed.lockdown) { + warnx("UAVCAN command bridge: system armed, exiting now."); + break; + } } } From 2a7e75043d0017e3462d6a3c305b047180136d67 Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Sun, 11 Oct 2015 23:45:15 +1100 Subject: [PATCH 04/42] Reduced logging verbosity; fixed stack size; fixed ESC indexes --- src/modules/uavcan/uavcan_servers.cpp | 51 ++++++++++++--------------- src/modules/uavcan/uavcan_servers.hpp | 12 +++---- 2 files changed, 28 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index e9eb573d36..38cc01f0c8 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -61,7 +61,7 @@ #include #include -#include +#include ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); @@ -369,7 +369,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } else { _param_in_progress = true; _param_index = request.param_index; - warnx("UAVCAN command bridge: sent GetSet"); } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { uavcan::protocol::param::GetSet::Request req; @@ -393,7 +392,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } else { _param_in_progress = true; _param_index = request.param_index; - warnx("UAVCAN command bridge: sent GetSet"); } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { // This triggers the _param_list_in_progress case below. @@ -415,9 +413,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _param_list_node_id = get_next_active_node_id(1); _param_list_all_nodes = true; - warnx("UAVCAN command bridge: starting global param list"); + warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); - if (_param_counts[_param_list_node_id.get()] == 0) { + if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } } @@ -432,25 +430,27 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Handle parameter listing index/node ID advancement if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { - if (_param_index >= _param_counts[_param_list_node_id.get()]) { + if (_param_index >= _param_counts[_param_list_node_id]) { + warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); // Reached the end of the current node's parameter set. _param_list_in_progress = false; if (_param_list_all_nodes) { // We're listing all parameters for all nodes -- get the next node ID - uavcan::NodeID next_id = get_next_active_node_id(_param_list_node_id); - if (next_id != _param_list_node_id) { + uint8_t next_id = get_next_active_node_id(_param_list_node_id); + if (next_id < 128) { + _param_list_node_id = next_id; /* * If there is a next node ID, check if that node's parameters * have been counted before. If not, do it now. */ - if (_param_counts[_param_list_node_id.get()] == 0) { + if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } // Keep on listing. _param_index = 0; _param_list_in_progress = true; - warnx("UAVCAN command bridge: incrementing global param list node ID"); + warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); } } } @@ -466,10 +466,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(_param_list_node_id, req); if (call_res < 0) { _param_list_in_progress = false; - warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; - warnx("UAVCAN command bridge: sent GetSet during param list operation"); } } @@ -585,8 +584,6 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult(); } - warnx("UAVCAN command bridge: successful GetSet response for param %s, node %hhu", response.param_id, response.node_id); - if (_param_response_pub == nullptr) { _param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response); } else { @@ -670,19 +663,17 @@ void UavcanServers::param_count(uavcan::NodeID node_id) } } -uavcan::NodeID UavcanServers::get_next_active_node_id(const uavcan::NodeID &base) +uint8_t UavcanServers::get_next_active_node_id(uint8_t base) { - for (int i = base.get(); i < 128; i++) { - if (_node_info_retriever.isNodeKnown(i) && _subnode.getNodeID() != i) { - return uavcan::NodeID(i); - } - } + base++; + for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) || + _subnode.getNodeID().get() == base); base++); return base; } void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) { - uavcan::NodeID next_id = get_next_active_node_id(result.getCallID().server_node_id); + uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); if (!result.isSuccessful()) { warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); @@ -693,7 +684,7 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult(); esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index); - _esc_enumeration_index = std::max(_esc_enumeration_index, esc_index); + _esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1)); _esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get(); @@ -787,8 +778,10 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult SubNode; @@ -155,15 +155,15 @@ private: * * The node's UAVCAN ID is used as the index into the _param_counts array. */ - uint16_t _param_counts[128]; + uint8_t _param_counts[128]; bool _count_in_progress; - uint16_t _count_index; + uint8_t _count_index; bool _param_in_progress; - uint16_t _param_index; + uint8_t _param_index; bool _param_list_in_progress; bool _param_list_all_nodes; - uavcan::NodeID _param_list_node_id; + uint8_t _param_list_node_id; bool _cmd_in_progress; @@ -184,7 +184,7 @@ private: uavcan::ServiceClient _param_getset_client; void param_count(uavcan::NodeID node_id); - uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base); + uint8_t get_next_active_node_id(uint8_t base); bool _mutex_inited; volatile bool _check_fw; From bde79c80b268008c210f1723ba544ea8c9f4140f Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Tue, 13 Oct 2015 00:43:51 +1100 Subject: [PATCH 05/42] Added parameter save/erase support --- src/modules/uavcan/uavcan_servers.cpp | 165 ++++++++++++++++++-------- src/modules/uavcan/uavcan_servers.hpp | 12 +- 2 files changed, 126 insertions(+), 51 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 38cc01f0c8..284eeb53f6 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -105,9 +105,13 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) : _param_list_in_progress(false), _param_list_all_nodes(false), _param_list_node_id(1), + _param_dirty_bitmap{0, 0, 0, 0}, + _param_save_opcode(0), _cmd_in_progress(false), _param_response_pub(nullptr), _param_getset_client(_subnode), + _param_opcode_client(_subnode), + _param_restartnode_client(_subnode), _mutex_inited(false), _check_fw(false), _esc_enumeration_active(false), @@ -312,17 +316,13 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); + _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); + _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); - uavcan::ServiceClient restartnode_client(_subnode); - restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); - - uavcan::ServiceClient opcode_client(_subnode); - opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); - _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; memset(_param_counts, 0, sizeof(_param_counts)); @@ -386,6 +386,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) req.value.to() = request.int_value; } + // Set the dirty bit for this node + set_node_params_dirty(request.node_id); + int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); @@ -485,7 +488,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int node_id = static_cast(cmd.param2 + 0.5f); int call_res; - warnx("UAVCAN command bridge: received command ID %d, node ID %d", command_id, node_id); + warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); switch (command_id) { case 0: @@ -502,38 +505,33 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } break; } - case 2: { - // Command is a restart node request - uavcan::protocol::RestartNode::Request restart_req; - restart_req.magic_number = restart_req.MAGIC_NUMBER; - call_res = restartnode_client.call(node_id, restart_req); - if (call_res < 0) { - warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res); - } else { - _cmd_in_progress = true; - warnx("UAVCAN command bridge: sent RestartNode"); - } - break; - } - case 3: - case 4: { - // Command is a param save or erase request - uavcan::protocol::param::ExecuteOpcode::Request opcode_req; - opcode_req.opcode = command_id == 3 ? opcode_req.OPCODE_SAVE : opcode_req.OPCODE_ERASE; - call_res = opcode_client.call(node_id, opcode_req); - if (call_res < 0) { - warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res); - } else { - _cmd_in_progress = true; - warnx("UAVCAN command bridge: sent ExecuteOpcode"); - } - break; - } default: { warnx("UAVCAN command bridge: unknown command ID %d", command_id); break; } } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { + int command_id = static_cast(cmd.param1 + 0.5f); + + warnx("UAVCAN command bridge: received storage command ID %d", command_id); + + switch (command_id) { + case 1: { + // Param save request + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + param_opcode(get_next_dirty_node_id(1)); + break; + } + case 2: { + // Command is a param erase request -- apply it to all active nodes by setting the dirty bit + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; + for (int i = 1; i < 128; i = get_next_active_node_id(i)) { + set_node_params_dirty(i); + } + param_opcode(get_next_dirty_node_id(1)); + break; + } + } } } @@ -556,22 +554,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) return (pthread_addr_t) 0; } -void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) -{ - bool success = result.isSuccessful(); - uavcan::protocol::RestartNode::Response resp = result.getResponse(); - success &= resp.ok; - _cmd_in_progress = false; -} - -void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) -{ - bool success = result.isSuccessful(); - uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); - success &= resp.ok; - _cmd_in_progress = false; -} - void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) { if (_count_in_progress) { @@ -663,6 +645,82 @@ void UavcanServers::param_count(uavcan::NodeID node_id) } } +void UavcanServers::param_opcode(uavcan::NodeID node_id) +{ + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = _param_save_opcode; + int call_res = _param_opcode_client.call(node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent ExecuteOpcode"); + } +} + +void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; + + if (!result.isSuccessful()) { + warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id); + } else if (!result.getResponse().ok) { + warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id); + } else { + warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id); + + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + int call_res = _param_restartnode_client.call(node_id, restart_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res); + } else { + warnx("UAVCAN command bridge: sent RestartNode"); + _cmd_in_progress = true; + } + } + + if (!_cmd_in_progress) { + /* + * Something went wrong, so cb_restart is never going to be called as a result of this request. + * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node + * ID and keep processing here. The dirty bit on the current node is still set, so the + * save/erase attempt will occur when the next save/erase command is received over MAVLink. + */ + node_id = get_next_dirty_node_id(node_id); + if (node_id < 128) { + param_opcode(node_id); + } + } +} + +void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::RestartNode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; + + if (success) { + warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id); + // Clear the dirty flag + clear_node_params_dirty(node_id); + } else { + warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id); + } + + // Get the next dirty node ID and send the same command to it + node_id = get_next_dirty_node_id(node_id); + if (node_id < 128) { + param_opcode(node_id); + } +} + uint8_t UavcanServers::get_next_active_node_id(uint8_t base) { base++; @@ -671,6 +729,13 @@ uint8_t UavcanServers::get_next_active_node_id(uint8_t base) return base; } +uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base) +{ + base++; + for (; base < 128 && !are_node_params_dirty(base); base++); + return base; +} + void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) { uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 118246c240..248198ad36 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -92,7 +92,7 @@ class UavcanServers static constexpr unsigned QueuePoolSize = (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 5000; + static constexpr unsigned StackSize = 4500; static constexpr unsigned Priority = 120; typedef uavcan::SubNode SubNode; @@ -165,6 +165,9 @@ private: bool _param_list_all_nodes; uint8_t _param_list_node_id; + uint32_t _param_dirty_bitmap[4]; + uint8_t _param_save_opcode; + bool _cmd_in_progress; // uORB topic handle for MAVLink parameter responses @@ -182,9 +185,16 @@ private: void cb_restart(const uavcan::ServiceCallResult &result); uavcan::ServiceClient _param_getset_client; + uavcan::ServiceClient _param_opcode_client; + uavcan::ServiceClient _param_restartnode_client; void param_count(uavcan::NodeID node_id); + void param_opcode(uavcan::NodeID node_id); uint8_t get_next_active_node_id(uint8_t base); + uint8_t get_next_dirty_node_id(uint8_t base); + void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); } + void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); } + bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); } bool _mutex_inited; volatile bool _check_fw; From 148148b6a05a63413557c73169621ee6c64c8277 Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Tue, 13 Oct 2015 20:39:03 +1100 Subject: [PATCH 06/42] Update libuavcan and add missing PRI64 define --- Tools/generate_listener.py | 4 ++++ src/lib/uavcan | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 1ebb316baf..df46df3ade 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -111,6 +111,10 @@ print(""" #define PRIu64 "llu" #endif +#ifndef PRI64 +#define PRI64 "ll" +#endif + """) for m in messages: print("#include " % m) diff --git a/src/lib/uavcan b/src/lib/uavcan index 86c3397b1b..8effe93d6e 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 86c3397b1b423eeed86f21c8b08c8acfbd5add96 +Subproject commit 8effe93d6ee055d9169c1ba460064c90a50eddf1 From d071177dd6fb78f172979777359b06773e30be98 Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Tue, 13 Oct 2015 21:23:57 +1100 Subject: [PATCH 07/42] Added ROMFS firmware update support --- src/modules/uavcan/uavcan_servers.cpp | 279 ++++++++++++++++++++++++++ src/modules/uavcan/uavcan_servers.hpp | 8 +- 2 files changed, 286 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 284eeb53f6..8c3758e9af 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -308,6 +309,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) Lock lock(_subnode_mutex); + /* + Copy any firmware bundled in the ROMFS to the appropriate location on the + SD card, unless the user has copied other firmware for that device. + */ + unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); + /* the subscribe call needs to happen in the same thread, * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -861,3 +868,275 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResultd_type)) { + continue; + } + + // Make sure the path fits + size_t dev_dirname_len = strlen(dev_dirent->d_name); + size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len; + if (srcpath_dev_len > maxlen) { + warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name); + continue; + } + size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len; + if (dstpath_dev_len > maxlen) { + warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name); + continue; + } + + // Create the device name directory on the SD card if it doesn't already exist + dstpath[sd_path_len] = '/'; + memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); + + if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); + if (rv != 0) { + warnx("dev: couldn't create '%s'", dstpath); + continue; + } + } + + // Set up the source path + srcpath[romfs_path_len] = '/'; + memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); + + DIR* const dev_dir = opendir(srcpath); + if (!dev_dir) { + warnx("dev: couldn't open '%s'", srcpath); + continue; + } + + // Iterate over all version directories in the current ROMFS device directory + struct dirent* ver_dirent = NULL; + while ((ver_dirent = readdir(dev_dir)) != NULL) { + // Skip if not a directory + if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) { + continue; + } + + // Make sure the path fits + size_t ver_dirname_len = strlen(ver_dirent->d_name); + size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len; + if (srcpath_ver_len > maxlen) { + warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name); + continue; + } + size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len; + if (dstpath_ver_len > maxlen) { + warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name); + continue; + } + + // Create the device version directory on the SD card if it doesn't already exist + dstpath[dstpath_dev_len] = '/'; + memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); + + if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); + if (rv != 0) { + warnx("ver: couldn't create '%s'", dstpath); + continue; + } + } + + // Set up the source path + srcpath[srcpath_dev_len] = '/'; + memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); + + // Find the name of the bundled firmware file, or move on to the + // next directory if there's no file here. + DIR* const src_ver_dir = opendir(srcpath); + if (!src_ver_dir) { + warnx("ver: couldn't open '%s'", srcpath); + continue; + } + + struct dirent* src_fw_dirent = NULL; + while ((src_fw_dirent = readdir(src_ver_dir)) != NULL && + !DIRENT_ISFILE(src_fw_dirent->d_type)); + + if (!src_fw_dirent) { + (void)closedir(src_ver_dir); + continue; + } + + size_t fw_len = strlen(src_fw_dirent->d_name); + + bool copy_fw = true; + + // Clear out any romfs_ files in the version directory on the SD card + DIR* const dst_ver_dir = opendir(dstpath); + if (!dst_ver_dir) { + warnx("unlink: couldn't open '%s'", dstpath); + } else { + struct dirent* fw_dirent = NULL; + while ((fw_dirent = readdir(dst_ver_dir)) != NULL) { + // Skip if not a file + if (!DIRENT_ISFILE(fw_dirent->d_type)) { + continue; + } + + if (!memcmp(&fw_dirent->d_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) { + /* + * Exact match between SD card filename and ROMFS filename; must be the same version + * so don't bother deleting and rewriting it. + */ + copy_fw = false; + } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) { + size_t fw_len = strlen(fw_dirent->d_name); + size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + if (dstpath_fw_len > maxlen) { + // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator + warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + } else { + // File name starts with "romfs_", delete it. + dstpath[dstpath_ver_len] = '/'; + memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1); + unlink(dstpath); + + warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + } + } else { + // User file, don't copy firmware + copy_fw = false; + } + } + (void)closedir(dst_ver_dir); + } + + // If we need to, copy the file from ROMFS to the SD card + if (copy_fw) { + size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + + if (srcpath_fw_len > maxlen) { + warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); + } else if (dstpath_fw_len > maxlen) { + warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name); + } else { + // All OK, make the paths and copy the file + srcpath[srcpath_ver_len] = '/'; + memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); + + dstpath[dstpath_ver_len] = '/'; + memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX)); + memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1); + + rv = copyFw(dstpath, srcpath); + if (rv != 0) { + warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv); + } else { + warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath); + } + } + } + + (void)closedir(src_ver_dir); + } + + (void)closedir(dev_dir); + } + + (void)closedir(romfs_dir); +} + +int UavcanServers::copyFw(const char* dst, const char* src) { + int rv = 0; + int dfd, sfd; + uint8_t buffer[512]; + + dfd = open(dst, O_WRONLY | O_CREAT, 0666); + if (dfd < 0) { + warnx("copyFw: couldn't open dst"); + return -errno; + } + + sfd = open(src, O_RDONLY, 0); + if (sfd < 0) { + (void)close(dfd); + warnx("copyFw: couldn't open src"); + return -errno; + } + + ssize_t size = 0; + do { + size = read(sfd, buffer, sizeof(buffer)); + if (size < 0) { + warnx("copyFw: couldn't read"); + rv = -errno; + } else if (size > 0) { + rv = 0; + ssize_t remaining = size; + ssize_t total_written = 0; + ssize_t written = 0; + do { + written = write(dfd, &buffer[total_written], remaining); + if (written < 0) { + warnx("copyFw: couldn't write"); + rv = -errno; + } else { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + } + } while (rv == 0 && size != 0); + + (void)close(dfd); + (void)close(sfd); + + return rv; +} diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 248198ad36..0b83baa941 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -69,6 +69,9 @@ #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_ROMFS_FW_PATH "/etc/uavcan/fw" +#define UAVCAN_ROMFS_FW_PREFIX "romfs_" +#define UAVCAN_MAX_PATH_LENGTH (128 + 40) #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" /** @@ -92,7 +95,7 @@ class UavcanServers static constexpr unsigned QueuePoolSize = (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 4500; + static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; typedef uavcan::SubNode SubNode; @@ -221,4 +224,7 @@ private: uavcan::ServiceClient _enumeration_client; uavcan::ServiceClient _enumeration_getset_client; uavcan::ServiceClient _enumeration_save_client; + + void unpackFwFromROMFS(const char* sd_path, const char* romfs_path); + int copyFw(const char* dst, const char* src); }; From 7dacb835b9065e503af5c38da1d574b6c67dcd3c Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Wed, 14 Oct 2015 00:31:07 +1100 Subject: [PATCH 08/42] Fixing build on targets without the UAVCAN module --- Tools/generate_listener.py | 2 +- src/modules/mavlink/mavlink_parameters.cpp | 3 +++ src/modules/uavcan/uavcan_servers.cpp | 3 --- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index df46df3ade..124d9908eb 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -112,7 +112,7 @@ print(""" #endif #ifndef PRI64 -#define PRI64 "ll" +#define PRI64 "lld" #endif """) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index bdaa483dfc..747b21f6a9 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -47,6 +47,9 @@ #include "mavlink_parameters.h" #include "mavlink_main.h" +ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); +ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); + MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 8c3758e9af..713a4e00f7 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -64,9 +64,6 @@ #include -ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); -ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); - //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 From 92ae41c1cf970b5ec9fd7b1a9f919c305fcf2133 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 13:58:36 +0200 Subject: [PATCH 09/42] PX4 log: reduce output verbosity to save flash --- src/platforms/px4_log.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 5b7a65eeb6..2a0ff33773 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -357,8 +357,8 @@ __END_DECLS /**************************************************************************** * Non-verbose settings for a Release build to minimize strings in build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, 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__) @@ -366,9 +366,9 @@ __END_DECLS /**************************************************************************** * Medium verbose settings for a default build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) -#define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif From ae65269de9f0ea1e63ba5addce1e1788c62c93ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:06:42 +0200 Subject: [PATCH 10/42] UAVCAN: Increase stack since its hitting almost the limit --- src/modules/uavcan/uavcan_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 2b5e01e246..6a1d2f391d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,7 +94,7 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 1600; + static constexpr unsigned StackSize = 1800; public: typedef uavcan::Node Node; From cff6cde47b65d3470f286a0a3099da7b2f8935f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:07:17 +0200 Subject: [PATCH 11/42] UAVCAN: Set default optimization level to -Os because comm protocols should not hog the flash or RAM. --- src/modules/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index 3c7ad32998..c2bf52247d 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -63,7 +63,7 @@ px4_add_module( COMPILE_FLAGS -Wframe-larger-than=1500 -Wno-deprecated-declarations - -O3 + -Os SRCS # Main uavcan_main.cpp From 21431cf237a3da38004251d66f49082eb15bb60b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:08:03 +0200 Subject: [PATCH 12/42] EKF: Fix output commands, fix stack size for commandline tool so we do not run out of space during debug printing --- src/modules/ekf_att_pos_estimator/CMakeLists.txt | 1 + .../ekf_att_pos_estimator_main.cpp | 13 ++++++------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index 6f109def1f..fc7fefd2bf 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -39,6 +39,7 @@ endif() px4_add_module( MODULE modules__ekf_att_pos_estimator MAIN ekf_att_pos_estimator + STACK 3000 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS ekf_att_pos_estimator_main.cpp 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 afb7cfbac5..48d30ceac8 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 @@ -1163,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], @@ -1342,21 +1343,21 @@ void AttitudePositionEstimatorEKF::pollData() static hrt_abstime lastprint = 0; if (hrt_elapsed_time(&lastprint) > 1000000) { - warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); - warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); - warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), (double)(_sensor_combined.accelerometer_integral_m_s[0]), (double)(_sensor_combined.accelerometer_integral_m_s[1]), (double)(_sensor_combined.accelerometer_integral_m_s[2])); - warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), @@ -1666,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - PX4_INFO("running"); - estimator::g_estimator->print_status(); return 0; @@ -1692,6 +1691,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - PX4_ERR("unrecognized command"); + PX4_WARN("unrecognized command"); return 1; } From 88a4d0deca60d52d02daea09ab508eb03e793956 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:27:49 +0200 Subject: [PATCH 13/42] EKF: Reduce excessive stack size --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 48d30ceac8..615ad143aa 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 @@ -1122,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4200, + 3900, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); From cde947a1d88bdad89210891c1153d828073eca39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:44:37 +0200 Subject: [PATCH 14/42] FMUv2: Reduce excessive work task stack sizes --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 871b214f49..3340fdd0c9 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_WORKSTACKSIZE=1600 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From b83f56bc1d77cd67499c60d5e86f0d4435282a99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 17:31:09 +0200 Subject: [PATCH 15/42] sdlog2: Allocate log buffer as needed, not on start. This allows the system to leverage the log buffer RAM for UAVCAN first --- src/modules/sdlog2/logbuffer.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 9e25dd2f23..2768994dde 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size) lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; - lb->data = malloc(lb->size); - return (lb->data == 0) ? PX4_ERROR : PX4_OK; + lb->data = NULL; + return PX4_OK; } int logbuffer_count(struct logbuffer_s *lb) @@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb) bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) { + // allocate buffer if not yet present + if (lb->data == NULL) { + lb->data = malloc(lb->size); + } + + // allocation failed, bail out + if (lb->data == NULL) { + return false; + } + // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; From 110a630cbd01b18163dfca9c53361711584c2a49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 18:06:28 +0200 Subject: [PATCH 16/42] Fix PX4 log to show error --- src/platforms/px4_log.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 2a0ff33773..55bf3a55bc 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current; __END_DECLS // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME -#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN +#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR /**************************************************************************** * Implementation of log section formatting based on printf From 626edc6a8d593a9f1a1d28e836cb6e72e3410fd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 18:06:41 +0200 Subject: [PATCH 17/42] EKF: Add error macro when running into errors --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 615ad143aa..861b56c60f 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 @@ -1691,6 +1691,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - PX4_WARN("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } From 75b356478927c0aeb8cd0ac71f451f1e763856cf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:52:09 -0400 Subject: [PATCH 18/42] cmake better nuttx copy --- cmake/nuttx/px4_impl_nuttx.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 2586478383..db8f11ef00 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -213,8 +213,8 @@ function(px4_nuttx_add_export) # copy add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG} - COMMAND ${RM} -rf ${nuttx_src} - COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src} + COMMAND ${MKDIR} -p ${nuttx_src} + COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/ COMMAND ${RM} -rf ${nuttx_src}/.git COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp DEPENDS ${DEPENDS}) From 8e039c40790ff18c41d4bd0d3529f51407c7f673 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:52:45 -0400 Subject: [PATCH 19/42] cmake uavcan remove whitespace --- src/modules/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index c2bf52247d..b8c899840a 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -81,7 +81,7 @@ px4_add_module( DEPENDS platforms__common - uavcan + uavcan ) ## vim: set noet ft=cmake fenc=utf-8 ff=unix : From 77f2295b62513d77878c6f1726e0e86b807ed3c9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:53:55 -0400 Subject: [PATCH 20/42] cmake restore -Wbad-function-cast --- cmake/common/px4_base.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 9a0b17b7a2..6853b4f161 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -543,6 +543,7 @@ function(px4_add_common_flags) endif() set(c_warnings + -Wbad-function-cast -Wstrict-prototypes -Wmissing-prototypes -Wnested-externs From adc59b9e7186100022c3f177cc9895872bde59a9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 11:05:51 -0400 Subject: [PATCH 21/42] cmake improve submodule handling --- Makefile | 23 ++---- Tools/check_submodules.sh | 144 ------------------------------------ cmake/common/px4_base.cmake | 13 ++-- 3 files changed, 14 insertions(+), 166 deletions(-) delete mode 100755 Tools/check_submodules.sh diff --git a/Makefile b/Makefile index 2b9a34553f..e2f1814f0c 100644 --- a/Makefile +++ b/Makefile @@ -89,7 +89,7 @@ endif # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef @@ -104,13 +104,13 @@ endef # -------------------------------------------------------------------- # Do not put any spaces between function arguments. -px4fmu-v1_default: git-init +px4fmu-v1_default: $(call cmake-build,nuttx_px4fmu-v1_default) -px4fmu-v2_default: git-init +px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v2_simple: git-init +px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) nuttx_sim_simple: @@ -172,19 +172,8 @@ check_format: clean: @rm -rf build_*/ - -distclean: clean - @cd NuttX - @git clean -d -f -x - @cd .. - @cd src/modules/uavcan/libuavcan - @git clean -d -f -x - @cd ../../../.. - -# XXX this is not the right way to fix it, but we need a temporary solution -# for average joe -git-init: - @git submodule update --init --recursive + @(cd NuttX && git clean -d -f -x) + @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh deleted file mode 100755 index 5fa0831438..0000000000 --- a/Tools/check_submodules.sh +++ /dev/null @@ -1,144 +0,0 @@ -#!/bin/sh - -[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { - # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules - echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - -if [ -d NuttX/nuttx ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked NuttX submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo " NuttX sub repo not at correct version. Try 'git submodule update'" - echo " or follow instructions on http://pixhawk.org/dev/git/submodules" - echo "" - echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d mavlink/include/mavlink/v1.0 ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked mavlink submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d uavcan ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked uavcan submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "uavcan sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d src/lib/eigen ] -then - echo "ARG = $1" - if [ $1 = "qurt" ] - then - # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 - STATUSRETVAL=$(true) - else - 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 -fi - -if [ -d Tools/gencpp ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked gencpp submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "gencpp sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d Tools/genmsg ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked genmsg submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "genmsg sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -exit 0 diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 6853b4f161..086f77070f 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -136,14 +136,17 @@ function(px4_add_git_submodule) REQUIRED TARGET PATH ARGN ${ARGN}) string(REPLACE "/" "_" NAME ${PATH}) - add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMAND git submodule update --init --recursive -f ${PATH} - COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + COMMAND git submodule init ${PATH} + COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules ) add_custom_target(${TARGET} - DEPENDS git_${NAME}.stamp - ) + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMAND git submodule update --recursive ${PATH} + DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + ) endfunction() #============================================================================= From e2fda0154540f5f1d87287dd5861a2335c114eab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 12:19:39 -0400 Subject: [PATCH 22/42] nuttx only show stderr -failures will still be displayed, but we don't need to see normal successful compile output --- cmake/nuttx/px4_impl_nuttx.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index db8f11ef00..af7391f45c 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -229,7 +229,7 @@ function(px4_nuttx_add_export) COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh COMMAND ${ECHO} Exporting NuttX for ${CONFIG} - COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export + COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG}) From ca5e1bd62b2a15fe202ae914a42bb35d115b6c56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:36:55 +0200 Subject: [PATCH 23/42] Drivers: Add calibration check IOCTL, not implemented yet in sensor drivers. --- src/drivers/drv_sensor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a402f327b7..0c1ab48267 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -121,5 +121,10 @@ */ #define SENSORIOCGROTATION _SENSORIOC(6) +/** + * Test the sensor calibration + */ +#define SENSORIOCCALTEST _SENSORIOC(7) + #endif /* _DRV_SENSOR_H */ From 7238916d035d6550d8a614d865da90e97cb2c706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:37:18 +0200 Subject: [PATCH 24/42] Sensors: Reload calibration whenever a new sensor instance is detected --- src/modules/sensors/sensors.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 60e1a9aabb..61eab348d7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2020,6 +2020,11 @@ Sensors::task_main() * do subscriptions */ + int gcount_prev = _gyro_count; + int mcount_prev = _mag_count; + int acount_prev = _accel_count; + int bcount_prev = _baro_count; + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); @@ -2032,6 +2037,15 @@ Sensors::task_main() _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); + if (gcount_prev != _gyro_count || + mcount_prev != _mag_count || + acount_prev != _accel_count || + bcount_prev != _baro_count) { + + /* reload calibration params */ + parameter_update_poll(true); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); From a7c6a343c6ea0d27f007ee250415ea3bef81514e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:37:56 +0200 Subject: [PATCH 25/42] Commander: Do not enforce sensor order, only enforce that all present sensors need to be calibrated. --- src/modules/commander/PreflightCheck.cpp | 63 +++++++++++++++++------- 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index bbf5f8ec6b..883a851f26 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -71,6 +71,45 @@ namespace Commander { + +static int check_calibration(int fd, const char* param_template); + +int check_calibration(int fd, const char* param_template) +{ + bool calibration_found; + + /* new style: ask device for calibration state */ + ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + + calibration_found = (ret == OK); + + /* old style transition: check param values */ + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + while (!calibration_found) { + sprintf(s, param_template, instance); + param_t parm = param_find(s); + + /* if the calibration param is not present, abort */ + if (parm == PARAM_INVALID) { + break; + } + + /* if param get succeeds */ + int calibration_devid; + if (!param_get(parm, &(calibration_devid))) { + + /* if the devid matches, exit early */ + if (devid == calibration_devid) { + calibration_found = true; + break; + } + } + } + + return !calibration_found; +} + static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) { bool success = true; @@ -88,13 +127,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_MAG%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_MAG%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; @@ -132,13 +167,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_ACC%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_ACC%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; @@ -201,13 +232,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_GYRO%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_GYRO%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; From e5bb1cff91cb7537c3ca99739a406a65a5fb6948 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 14:13:39 +0200 Subject: [PATCH 26/42] Store primary sensor ID to allow cross-check of calibration on next boot --- src/modules/commander/PreflightCheck.cpp | 67 +++++++++++++++++-- .../commander/accelerometer_calibration.cpp | 12 ++++ src/modules/commander/gyro_calibration.cpp | 14 ++++ src/modules/commander/mag_calibration.cpp | 13 ++++ 4 files changed, 101 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 883a851f26..61aa461a91 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -110,7 +110,7 @@ int check_calibration(int fd, const char* param_template) return !calibration_found; } -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -338,50 +338,107 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- MAG ---- */ if (checkMag) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_MAG_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); + int device_id; - if (!magnometerCheck(mavlink_fd, i, !required) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- ACCEL ---- */ if (checkAcc) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_ACC_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); + int device_id - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- GYRO ---- */ if (checkGyro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_GYRO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); + int device_id; - if (!gyroCheck(mavlink_fd, i, !required) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- BARO ---- */ if (checkBaro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_BARO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); + int device_id; - if (!baroCheck(mavlink_fd, i, !required) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } } + + // TODO there is no logic in place to calibrate the primary baro yet + // // check if the primary device is present + // if (!prime_found) { + // failed = true; + // } } /* ---- AIRSPEED ---- */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 6a38d54cb6..2c52489fdf 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -173,6 +173,8 @@ int do_accel_calibration(int mavlink_fd) { int fd; int32_t device_id[max_accel_sens]; + int device_prio_max = 0; + int32_t device_id_primary = 0; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -259,6 +261,8 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; + failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary))); + /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); @@ -370,6 +374,14 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; + + // Get priority + int32_t prio = orb_priority(work_data.subs[i]); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_id[i]; + } } if (result == calibrate_return_ok) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 817cbcdb0e..6c8c840713 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, // z scale }; + int device_prio_max = 0; + int32_t device_id_primary = 0; + for (unsigned s = 0; s < max_gyros; s++) { char str[30]; @@ -199,6 +202,14 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + + // Get priority + int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = worker_data.device_id[s]; + } } int cancel_sub = calibrate_cancel_subscribe(); @@ -258,9 +269,12 @@ int do_gyro_calibration(int mavlink_fd) } if (res == OK) { + /* set offset parameters to new values */ bool failed = false; + failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary))); + for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { char str[30]; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4511882c7b..c9bd48a5cc 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -109,6 +109,8 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each int32_t device_ids[max_mags]; + int device_prio_max = 0; + int32_t device_id_primary = 0; char str[30]; for (size_t i=0; i device_prio_max) { + device_prio_max = prio; + device_id_primary = device_ids[cur_mag]; + } } } } @@ -550,6 +560,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } if (result == calibrate_return_ok) { + + (void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary)); + for (unsigned cur_mag=0; cur_mag Date: Tue, 6 Oct 2015 19:44:17 +0200 Subject: [PATCH 27/42] Commander: Finish preflight update for prime sensor IDs --- src/modules/commander/PreflightCheck.cpp | 30 ++++++++++++++----- .../commander/accelerometer_calibration.cpp | 12 ++++---- src/modules/commander/gyro_calibration.cpp | 5 ++-- src/modules/commander/mag_calibration.cpp | 12 ++++---- 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 61aa461a91..cdddd3b637 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -79,13 +79,16 @@ int check_calibration(int fd, const char* param_template) bool calibration_found; /* new style: ask device for calibration state */ - ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); calibration_found = (ret == OK); /* old style transition: check param values */ int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + char s[20]; + int instance = 0; + while (!calibration_found) { sprintf(s, param_template, instance); param_t parm = param_find(s); @@ -105,6 +108,7 @@ int check_calibration(int fd, const char* param_template) break; } } + instance++; } return !calibration_found; @@ -150,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) { bool success = true; @@ -215,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -255,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -272,6 +276,18 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } + // TODO: There is no baro calibration yet, since no external baros exist + // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + + // if (ret) { + // mavlink_and_console_log_critical(mavlink_fd, + // "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); + // success = false; + // goto out; + // } + +//out: + px4_close(fd); return success; } @@ -371,7 +387,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - int device_id + int device_id; if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; @@ -436,9 +452,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - // if (!prime_found) { + if (!prime_found) { // failed = true; - // } + } } /* ---- AIRSPEED ---- */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 2c52489fdf..d7a58e6254 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -156,6 +156,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; +static int32_t device_id[max_accel_sens]; +static int device_prio_max = 0; +static int32_t device_id_primary = 0; + calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -172,9 +176,6 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_accel_sens]; - int device_prio_max = 0; - int32_t device_id_primary = 0; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -261,7 +262,7 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); @@ -376,7 +377,8 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel timestamps[i] = arp.timestamp; // Get priority - int32_t prio = orb_priority(work_data.subs[i]); + int32_t prio; + orb_priority(worker_data.subs[i], &prio); if (prio > device_prio_max) { device_prio_max = prio; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 6c8c840713..54866d8cb3 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -204,7 +204,8 @@ int do_gyro_calibration(int mavlink_fd) worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); // Get priority - int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]); + int32_t prio; + orb_priority(worker_data.gyro_sensor_sub[s], &prio); if (prio > device_prio_max) { device_prio_max = prio; @@ -273,7 +274,7 @@ int do_gyro_calibration(int mavlink_fd) /* set offset parameters to new values */ bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index c9bd48a5cc..eba8feec99 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +int32_t device_ids[max_mags]; +int device_prio_max = 0; +int32_t device_id_primary = 0; + calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine @@ -108,9 +112,6 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each - int32_t device_ids[max_mags]; - int device_prio_max = 0; - int32_t device_id_primary = 0; char str[30]; for (size_t i=0; i device_prio_max) { device_prio_max = prio; @@ -561,7 +563,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag if (result == calibrate_return_ok) { - (void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary)); + (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); for (unsigned cur_mag=0; cur_mag Date: Tue, 6 Oct 2015 19:44:50 +0200 Subject: [PATCH 28/42] sensors app: Finish preflight update --- src/modules/sensors/CMakeLists.txt | 1 - src/modules/sensors/sensor_params.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index fdfc2dd774..e296ebe103 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( -O3 SRCS sensors.cpp - sensor_params.c DEPENDS platforms__common ) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1db6ed1b77..393cd63a06 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); +/** + * Primary accel ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0); + +/** + * Primary gyro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); + +/** + * Primary mag ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); + +/** + * Primary baro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); + /** * Differential pressure sensor offset * From a3f207f515715677a93b545851309850bc9f207c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 13:25:06 -0400 Subject: [PATCH 29/42] cmake fix windows upload --- cmake/common/px4_base.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 086f77070f..5c9b8aefdf 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -421,7 +421,8 @@ function(px4_add_upload) ) elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows") foreach(port RANGE 32 0) - list(APPEND "COM${port}") + list(APPEND serial_ports + "COM${port}") endforeach() endif() px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",") From 336ca86117f981377879189265ca93ca7aaa8437 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:02:02 +0200 Subject: [PATCH 30/42] Commander: Ensure primary sensor is present if configured --- src/modules/commander/PreflightCheck.cpp | 49 ++++++++++++++---------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index cdddd3b637..2a8a0695cf 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -72,9 +72,9 @@ namespace Commander { -static int check_calibration(int fd, const char* param_template); +static int check_calibration(int fd, const char* param_template, int &devid); -int check_calibration(int fd, const char* param_template) +int check_calibration(int fd, const char* param_template, int &devid) { bool calibration_found; @@ -82,13 +82,13 @@ int check_calibration(int fd, const char* param_template) int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); calibration_found = (ret == OK); - - /* old style transition: check param values */ - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); char s[20]; int instance = 0; + /* old style transition: check param values */ while (!calibration_found) { sprintf(s, param_template, instance); param_t parm = param_find(s); @@ -131,7 +131,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in return false; } - int ret = check_calibration(fd, "CAL_MAG%u_ID"); + int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -171,7 +171,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int ret = check_calibration(fd, "CAL_ACC%u_ID"); + int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -236,7 +236,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } - int ret = check_calibration(fd, "CAL_GYRO%u_ID"); + int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -276,6 +276,8 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } + device_id = -1000; + // TODO: There is no baro calibration yet, since no external baros exist // int ret = check_calibration(fd, "CAL_BARO%u_ID"); @@ -361,7 +363,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); - int device_id; + int device_id = -1; if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -373,8 +375,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + failed = true; } } @@ -387,7 +390,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - int device_id; + int device_id = -1; if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; @@ -399,8 +402,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + failed = true; } } @@ -413,7 +417,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); - int device_id; + int device_id = -1; if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -425,8 +429,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + failed = true; } } @@ -439,7 +444,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); - int device_id; + int device_id = -1; if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -452,8 +457,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - if (!prime_found) { - // failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + failed = true; } } @@ -467,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd) != OK) { + mavlink_log_critical(mavlink_fd, "RC calibration check failed"); failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ - if(checkGNSS) { + if (checkGNSS) { if(!gnssCheck(mavlink_fd)) { failed = true; } From 53bbe1920d635918f50985ccd5a2fea4482fc28a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:02:27 +0200 Subject: [PATCH 31/42] Navigator: Make error message less cryptic --- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index a31aa25805..880d4bd98e 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } else { /* item is too far from home */ - mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp); warning_issued = true; return false; } From dbcb226de6ffd41882342eeb65f9bb1745641751 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:11:00 +0200 Subject: [PATCH 32/42] Sensors: Ensure all mag rotations are visible to the GCS --- src/modules/sensors/sensors.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 61eab348d7..6009e2dd5c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ID", i); int device_id; failed = failed || (OK != param_get(param_find(str), &device_id)); + (void)sprintf(str, "CAL_MAG%u_ROT", i); + (void)param_find(str); if (failed) { px4_close(fd); From 02a21aac04002d255acbbe0379e307b0c8fcad72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 22:31:56 +0200 Subject: [PATCH 33/42] Fix signed compare for CLANG --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6009e2dd5c..c14afb322f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2022,10 +2022,10 @@ Sensors::task_main() * do subscriptions */ - int gcount_prev = _gyro_count; - int mcount_prev = _mag_count; - int acount_prev = _accel_count; - int bcount_prev = _baro_count; + unsigned gcount_prev = _gyro_count; + unsigned mcount_prev = _mag_count; + unsigned acount_prev = _accel_count; + unsigned bcount_prev = _baro_count; _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); From e20c683896699b57793051c6f32b73ed0801887a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 22:34:24 +0200 Subject: [PATCH 34/42] Navigator: Advoid no symbols warning --- src/modules/navigator/CMakeLists.txt | 7 ------- src/modules/navigator/datalinkloss_params.c | 4 ---- src/modules/navigator/geofence_params.c | 4 ---- src/modules/navigator/gpsfailure_params.c | 4 ---- src/modules/navigator/mission_params.c | 4 ---- src/modules/navigator/navigator_params.c | 4 ---- src/modules/navigator/rcloss_params.c | 4 ---- src/modules/navigator/rtl_params.c | 4 ---- 8 files changed, 35 deletions(-) diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 177f172755..d8399ccafd 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -39,24 +39,17 @@ px4_add_module( -Os SRCS navigator_main.cpp - navigator_params.c navigator_mode.cpp mission_block.cpp mission.cpp - mission_params.c loiter.cpp rtl.cpp - rtl_params.c mission_feasibility_checker.cpp geofence.cpp - geofence_params.c datalinkloss.cpp - datalinkloss_params.c rcloss.cpp - rcloss_params.c enginefailure.cpp gpsfailure.cpp - gpsfailure_params.c DEPENDS platforms__common ) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index a1ba7869ed..9eb0299ab7 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Data Link Loss parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 7c1d8a266a..63eec7213d 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Geofence parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index ca2f540505..6c835d062e 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * GPS Failure Navigation Mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index b2094f62e3..7c2942f53e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * Mission parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index e1d29334c6..17e580c664 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -40,10 +40,6 @@ * @author Thomas Gubler */ -#include - -#include - /** * Loiter radius (FW only) * diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 3c9a6e40ef..958dab9266 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * OBC RC Loss mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index a33ded28ab..e65a41b775 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * RTL parameters, accessible via MAVLink */ From ee77ff4830ffc42d63cd852bb925aa0be8e5fd65 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 22:36:01 +0200 Subject: [PATCH 35/42] FW Att control: Avoid no symbols warnings --- src/modules/fw_att_control/CMakeLists.txt | 1 - src/modules/fw_att_control/fw_att_control_params.c | 5 ----- 2 files changed, 6 deletions(-) diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index b434a36a86..b093c71cee 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -38,7 +38,6 @@ px4_add_module( -Os SRCS fw_att_control_main.cpp - fw_att_control_params.c DEPENDS platforms__common ) 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 5e640fcc12..51c06fabb9 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -40,11 +40,6 @@ * @author Thomas Gubler */ -#include - -#include - - /* * Controller parameters, accessible via MAVLink * From 3105b49b22e25c5a74976669f4aa3df92b92a37d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 22:36:19 +0200 Subject: [PATCH 36/42] FW Pos control: Avoid no symbols warnings --- src/modules/fw_pos_control_l1/CMakeLists.txt | 2 -- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 3 --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 3 --- 3 files changed, 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 78313eed84..e74b6e4b37 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -39,11 +39,9 @@ px4_add_module( -Os SRCS fw_pos_control_l1_main.cpp - fw_pos_control_l1_params.c landingslope.cpp mtecs/mTecs.cpp mtecs/limitoverride.cpp - mtecs/mTecs_params.c DEPENDS platforms__common lib__external_lgpl diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index e06814d5f3..cfd1988f44 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -39,9 +39,6 @@ * @author Lorenz Meier */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 3b98454b95..0d1025f7d4 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -39,9 +39,6 @@ * @author Thomas Gubler */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ From a3552896cae979f1ff7a19731030d68a66844990 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Oct 2015 09:32:13 +0200 Subject: [PATCH 37/42] Fix bug in accel integrator handling in mavlink interface --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 88123b3e45..d0b95b7f89 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1444,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt; - hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt; - hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f; + hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f; memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? From 73e9d27acfcf596e21eadbfa7466dae2389de0dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Oct 2015 11:13:24 +0200 Subject: [PATCH 38/42] Fix incorrect addition of G to attitude EKF --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 861b56c60f..61bd32a1b3 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 @@ -1267,7 +1267,7 @@ void AttitudePositionEstimatorEKF::pollData() } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f; + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; From bb2e5b7c84b7bc21b8a7355be0103d1cd4b71d26 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Oct 2015 12:31:56 +0200 Subject: [PATCH 39/42] Update listener to also support double precision --- Tools/generate_listener.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 88f7edcdb6..1f240ddbf6 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -24,11 +24,16 @@ for index,m in enumerate(raw_messages): if ('float32[' in line.split(' ')[0]): num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) - if ('uint64[' in line.split(' ')[0]): + elif ('float64[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("double_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif ('uint64[' in line.split(' ')[0]): num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) elif(line.split(' ')[0] == "float32"): temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "float64"): + temp_list.append(("double",line.split(' ')[1].split('\t')[0].split('\n')[0])) elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1: temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1: @@ -155,11 +160,19 @@ for index,m in enumerate(messages[1:]): print("\t\t\torb_copy(ID,sub,&container);") for item in message_elements[index+1]: if item[0] == "float": - print("\t\t\tprintf(\"%s: %%f\\n\",(double)container.%s);" % (item[1], item[1])) + print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1])) elif item[0] == "float_array": print("\t\t\tprintf(\"%s: \");" % item[1]) print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2]) - print("\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1]) + print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1]) + print("\t\t\t}") + print("\t\t\tprintf(\"\\n\");") + elif item[0] == "double": + print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1])) + elif item[0] == "double_array": + print("\t\t\tprintf(\"%s: \");" % item[1]) + print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2]) + print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1]) print("\t\t\t}") print("\t\t\tprintf(\"\\n\");") elif item[0] == "uint64": From 99c1ae12eb4fb47386a1fb0885c351cb1a4a3a9b Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 15 Oct 2015 16:05:31 +0200 Subject: [PATCH 40/42] fix limiting of dt --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 df8773b169..53a7c678f6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -897,7 +897,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms + dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ From 7d63920f25b0e5e7d23b820fba7773385a197719 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Oct 2015 14:41:49 -0400 Subject: [PATCH 41/42] srcparser.py split mixed windows path with os.path.split -fixes #2997 --- Tools/px4airframes/rcout.py | 3 ++- Tools/px4airframes/srcparser.py | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py index de5af0f305..48bb1774a9 100644 --- a/Tools/px4airframes/rcout.py +++ b/Tools/px4airframes/rcout.py @@ -1,5 +1,6 @@ from xml.sax.saxutils import escape import codecs +import os class RCOutput(): def __init__(self, groups, board): @@ -30,7 +31,7 @@ class RCOutput(): for group in groups: result += "# GROUP: %s\n\n" % group.GetName() for param in group.GetParams(): - path = param.GetPath().rsplit('/', 1)[1] + path = os.path.split(param.GetPath())[1] id_val = param.GetId() name = param.GetFieldValue("short_desc") long_desc = param.GetFieldValue("long_desc") diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index fb491ede15..e1d522dcd8 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -1,5 +1,6 @@ import sys import re +import os class ParameterGroup(object): """ @@ -160,7 +161,7 @@ class SourceParser(object): """ airframe_id = None - airframe_id = path.rsplit('/',1)[1].split('_',1)[0] + airframe_id = os.path.split(path)[1].split('_',1)[0] # Skip if not numeric if (not self.IsNumber(airframe_id)): From 7ce0cbd8cb4f3811b19ad020e2096fb26eb0e11c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Oct 2015 09:51:31 +0200 Subject: [PATCH 42/42] Complete HW test instructions --- src/examples/hwtest/hwtest.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index a403f2fa24..97a525a8e9 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -57,8 +57,10 @@ __EXPORT int ex_hwtest_main(int argc, char *argv[]); int ex_hwtest_main(int argc, char *argv[]) { - warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); - warnx("(run to do so)"); + warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!"); + warnx("(run ,)"); + warnx("( and)"); + warnx("( to do so)"); warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators;