From 9ea62e74025316071651898037a400cdac564181 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 May 2015 21:01:38 +0200 Subject: [PATCH 01/16] mavlink: Enable a few helpful streams on companion link --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e5cddfafb6..b6050fa56b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1406,6 +1406,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); @@ -1414,6 +1417,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("RC_CHANNELS_RAW", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); From bc75814d500c673fa8699f8d242c88e610ecded2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 May 2015 21:02:28 +0200 Subject: [PATCH 02/16] Increase buffer sizes on companion link --- 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 ab8f9eef62..19e0f7c632 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -560,8 +560,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=2200 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 From fb4dc27bc9c604e71733b38ea06045c041cd2bc6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 10:26:36 +0200 Subject: [PATCH 03/16] commander: Improve user feedback on sensor health, in particular during calibration --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index cdcc12043e..2d631305dd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -219,6 +219,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Sensors need to be initialized for STANDBY state, except for HIL if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; @@ -232,7 +233,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } @@ -240,7 +241,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { From 9179fcefc9ce138f8fdbbe9e194ca11ecb116b33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 17:25:37 +0200 Subject: [PATCH 04/16] Calibration state machine fixes, generates less bogus error messages during calibration --- msg/vehicle_status.msg | 1 + src/modules/commander/commander.cpp | 10 ++++--- .../commander/state_machine_helper.cpp | 26 ++++++++++--------- 3 files changed, 22 insertions(+), 15 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 430f7dd768..c5d5ee9a16 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state +bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT. int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7e29dbd94d..3e9b1de6f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1604,7 +1604,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -2689,6 +2689,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; + } else { + status.calibration_enabled = true; } if ((int)(cmd.param1) == 1) { @@ -2748,12 +2750,14 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - calib_ret = OK; + calib_ret = OK; } /* this always succeeds */ calib_ret = OK; } + status.calibration_enabled = false; + if (calib_ret == OK) { tune_positive(true); @@ -2771,7 +2775,7 @@ void *commander_low_prio_loop(void *arg) status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); } else { tune_negative(true); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2d631305dd..b53eeebeb6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,17 +216,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - // Sensors need to be initialized for STANDBY state, except for HIL - if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && - (!status->condition_system_sensors_initialized)) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { @@ -244,8 +233,21 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { - + // Silent ignore + feedback_provided = true; } + + // Sensors need to be initialized for STANDBY state, except for HIL + } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && + (!status->condition_system_sensors_initialized)) { + if (!fRunPreArmChecks) { + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + } + feedback_provided = true; + valid_transition = false; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition From 05720b5aef53f265c9c364618abc202c3354a03f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 09:24:48 -0700 Subject: [PATCH 05/16] Added ability to set OS specific path for dataman file dataman can now be started using: dataman start -f filepath Signed-off-by: Mark Charlebois --- src/modules/dataman/dataman.c | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index e454568825..799723eeed 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -131,8 +131,8 @@ static sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; -// FIXME - need a configurable path that is not OS specific -static const char *k_data_manager_device_path = "/fs/microsd/dataman"; +static const char *default_device_path = "/fs/microsd/dataman"; +static char *k_data_manager_device_path = NULL; /* The data manager work queues */ @@ -831,7 +831,7 @@ stop(void) static void usage(void) { - warnx("usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); + warnx("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}"); } int @@ -848,11 +848,20 @@ dataman_main(int argc, char *argv[]) warnx("dataman already running"); return -1; } + if (argc == 4 && strcmp(argv[2],"-f") == 0) { + k_data_manager_device_path = strdup(argv[3]); + warnx("dataman file set to: %s\n", k_data_manager_device_path); + } + else { + k_data_manager_device_path = strdup(default_device_path); + } start(); if (g_fd < 0) { warnx("dataman start failed"); + free(k_data_manager_device_path); + k_data_manager_device_path = NULL; return -1; } @@ -866,8 +875,11 @@ dataman_main(int argc, char *argv[]) return -1; } - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { stop(); + free(k_data_manager_device_path); + k_data_manager_device_path = NULL; + } else if (!strcmp(argv[1], "status")) status(); else if (!strcmp(argv[1], "poweronrestart")) From 7da96b3bac49ca8fc391e0bb44c31b344c0818b0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 16:42:55 -0700 Subject: [PATCH 06/16] POSIX: Fixed uint64_t print in generated topic_listener.cpp Signed-off-by: Mark Charlebois --- Tools/generate_listener.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index e52d3cd26f..5cfe4a6281 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -91,6 +91,8 @@ print """ #include #include #include +#define __STDC_FORMAT_MACROS +#include """ for m in messages: print "#include " % m @@ -135,7 +137,7 @@ for index,m in enumerate(messages[1:]): print "\t\t\t}" print "\t\t\tprintf(\"\\n\");" elif item[0] == "uint64": - print "\t\t\tprintf(\"%s: %%lu\\n \",container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%\" PRIu64 \"\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "uint8": print "\t\t\tprintf(\"%s: %%u\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "bool": From dc1c4a30e1a8ae6f305bafc7e6da1da4fb631c95 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 20 May 2015 19:19:08 -0700 Subject: [PATCH 07/16] Makefile multi-target support NuttX is still the default target and all NuttX configs can still be built with: make Individual NuttX, POSIX, and QuRT configs can now be built more easily by specifying the target and configs: make posix posix_default make qurt qurt_hello make nuttx aerocore_default Running make with just the target will make all the configs for that target: make nuttx make qurt make posix The help is also target specific: make nuttx help make qurt help make posix help "make help" will still assume you want help for the NuttX target Added a new QuRT config called qurt_hello as a sample config to test buiding in different commands for separate configs. Signed-off-by: Mark Charlebois --- Makefile | 22 ++++-- makefiles/firmware_posix.mk | 2 +- makefiles/firmware_qurt.mk | 2 +- makefiles/qurt/config_qurt_hello.mk | 73 +++++++++++++++++++ .../qurt/px4_layer/commands_default.c | 68 +++++++++++++++++ src/platforms/qurt/px4_layer/commands_hello.c | 45 ++++++++++++ src/platforms/qurt/px4_layer/main.cpp | 30 ++------ src/platforms/qurt/px4_layer/module.mk | 8 +- 8 files changed, 216 insertions(+), 34 deletions(-) create mode 100644 makefiles/qurt/config_qurt_hello.mk create mode 100644 src/platforms/qurt/px4_layer/commands_default.c create mode 100644 src/platforms/qurt/px4_layer/commands_hello.c diff --git a/Makefile b/Makefile index 4a0f13cd19..3f78f32362 100644 --- a/Makefile +++ b/Makefile @@ -33,6 +33,12 @@ # Top-level Makefile for building PX4 firmware images. # +TARGETS := nuttx posix qurt +EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) +ifneq ($(EXPLICIT_TARGET),) + export PX4_TARGET_OS=$(EXPLICIT_TARGET) +endif + # # Get path and tool configuration # @@ -271,14 +277,14 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -posix: - make PX4_TARGET_OS=posix +nuttx: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -nuttx: - make PX4_TARGET_OS=nuttx +posix: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -qurt: - make PX4_TARGET_OS=qurt +qurt: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) posixrun: Tools/posix_run.sh @@ -327,9 +333,11 @@ help: @$(ECHO) " Available targets:" @$(ECHO) " ------------------" @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " archives" @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." @$(ECHO) "" +endif @$(ECHO) " all" @$(ECHO) " Build all firmware configs: $(CONFIGS)" @$(ECHO) " A limited set of configs can be built with CONFIGS=" @@ -342,6 +350,7 @@ help: @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " distclean" @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." @$(ECHO) "" @@ -350,6 +359,7 @@ help: @$(ECHO) " firmware to the board when the build is complete. Not supported for" @$(ECHO) " all configurations." @$(ECHO) "" +endif @$(ECHO) " testbuild" @$(ECHO) " Perform a complete clean build of the entire tree." @$(ECHO) "" diff --git a/makefiles/firmware_posix.mk b/makefiles/firmware_posix.mk index 6acec4e733..912e7bcc3f 100644 --- a/makefiles/firmware_posix.mk +++ b/makefiles/firmware_posix.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk index 41128fa60f..a367e4a327 100644 --- a/makefiles/firmware_qurt.mk +++ b/makefiles/firmware_qurt.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/qurt/config_qurt_hello.mk b/makefiles/qurt/config_qurt_hello.mk new file mode 100644 index 0000000000..4b11a79fe3 --- /dev/null +++ b/makefiles/qurt/config_qurt_hello.mk @@ -0,0 +1,73 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/qurt/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c new file mode 100644 index 0000000000..9d229d610f --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file hello.c + * Commands to run for the QuRT "default" target + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = + "hello start\n" + "uorb start\n" + "simulator start -s\n" + "barosim start\n" + "adcsim start\n" + "accelsim start\n" + "gyrosim start\n" + "list_devices\n" + "list_topics\n" + "list_tasks\n" + "param show *\n" + "rgbled start\n" +#if 0 + "sensors start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "hil mode_pwm" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "mavlink start -d /tmp/ttyS0\n" + "commander start\n" +#endif + ; + + return commands; +} diff --git a/src/platforms/qurt/px4_layer/commands_hello.c b/src/platforms/qurt/px4_layer/commands_hello.c new file mode 100644 index 0000000000..d38e73da0e --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_hello.c @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file hello.c + * Commands to run for the QuRT "hello" target + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = "hello start"; + + return commands; +} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 3f42c1c301..538b64229f 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,30 +50,10 @@ using namespace std; extern void init_app_map(map &apps); extern void list_builtins(map &apps); -static const char *commands = -"hello start\n" -"uorb start\n" -"simulator start -s\n" -"barosim start\n" -"adcsim start\n" -"accelsim start\n" -"gyrosim start\n" -"list_devices\n" -"list_topics\n" -"list_tasks\n" -"param show *\n" -"rgbled start\n" -#if 0 -"hil mode_pwm" -"param set CAL_GYRO0_ID 2293760\n" -"param set CAL_ACC0_ID 1310720\n" -"param set CAL_ACC1_ID 1376256\n" -"param set CAL_MAG0_ID 196608\n" -"sensors start\n" -"mavlink start -d /tmp/ttyS0\n" -"commander start\n" -#endif -; +__BEGIN_DECLS +// The commands to run are specified in a target file: commands_.c +extern const char *get_commands(void); +__END_DECLS static void run_cmd(map &apps, const vector &appargs) { // command is appargs[0] @@ -158,7 +138,7 @@ int main(int argc, char **argv) init_app_map(apps); px4::init_once(); px4::init(argc, argv, "mainapp"); - process_commands(apps, commands); + process_commands(apps, get_commands()); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 92f1c6abfb..714263edcb 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -52,6 +52,12 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp + main.cpp +ifeq ($(CONFIG),qurt_hello) +SRCS += commands_hello.c +endif +ifeq ($(CONFIG),qurt_default) +SRCS += commands_default.c +endif MAXOPTIMIZATION = -Os From f24d807b666929dd286bc3f7681a175ce5153586 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 20 May 2015 19:35:43 -0700 Subject: [PATCH 08/16] QuRT: Fixed file descriptions for command_.c Signed-off-by: Mark Charlebois --- src/platforms/qurt/px4_layer/commands_default.c | 4 ++-- src/platforms/qurt/px4_layer/commands_hello.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c index 9d229d610f..1c4c50f022 100644 --- a/src/platforms/qurt/px4_layer/commands_default.c +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -31,8 +31,8 @@ * ****************************************************************************/ /** - * @file hello.c - * Commands to run for the QuRT "default" target + * @file commands_default.c + * Commands to run for the "qurt_default" config * * @author Mark Charlebois */ diff --git a/src/platforms/qurt/px4_layer/commands_hello.c b/src/platforms/qurt/px4_layer/commands_hello.c index d38e73da0e..b2ef09b457 100644 --- a/src/platforms/qurt/px4_layer/commands_hello.c +++ b/src/platforms/qurt/px4_layer/commands_hello.c @@ -31,8 +31,8 @@ * ****************************************************************************/ /** - * @file hello.c - * Commands to run for the QuRT "hello" target + * @file commands_hello.c + * Commands to run for the "qurt_hello" config * * @author Mark Charlebois */ From 0c3681c8964f3fcd9a396bb202954a64805e8f00 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 10:26:45 -0700 Subject: [PATCH 09/16] Sensors: added missing px4_close and removed _exit() call The QuRT build was broken from the call to exit. While fixing the code to clean up before returning, I found a missed call to px4_close. Signed-off-by: Mark Charlebois --- src/modules/sensors/sensors.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acf1726984..34bcc88e50 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -922,16 +922,16 @@ Sensors::gyro_init() warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); return ERROR; - } else { - - /* set the gyro internal sampling rate to default rate */ - px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); - - /* set the driver to poll at default rate */ - px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - } + /* set the gyro internal sampling rate to default rate */ + px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + + /* set the driver to poll at default rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + + px4_close(fd); + return OK; } @@ -2086,8 +2086,12 @@ Sensors::task_main() } while (0); if (ret) { + warnx("Sensor initialization failed"); _sensors_task = -1; - _exit(ret); + if (_fd_adc >=0) { + close(_fd_adc); + _fd_adc = -1; + } return; } From cb5db8ec83cfa923331ae12ffa5cfecf76c07d8e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 10:37:15 -0700 Subject: [PATCH 10/16] dataman: Fixed file permissions on file creation In Linux the file was being created with incorrect permissions. Signed-off-by: Mark Charlebois --- src/modules/dataman/dataman.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 799723eeed..15c568735b 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -671,7 +671,12 @@ task_main(int argc, char *argv[]) } /* Open or create the data manager file */ - g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, 0x0777); + g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY +#ifdef __PX4_LINUX + // Open with read/write permission for user + , S_IRUSR | S_IWUSR +#endif + ); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); From afa8266255eeec47dbc6617aa172723121b1da63 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 21 May 2015 15:30:22 +0200 Subject: [PATCH 11/16] do not update mission if it has unsupported mission items --- src/modules/navigator/mission.cpp | 7 +++- .../navigator/mission_feasibility_checker.cpp | 42 ++++++++++++++++++- .../navigator/mission_feasibility_checker.h | 1 + 3 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c1763ba93d..a68f4de012 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -207,6 +207,8 @@ Mission::update_onboard_mission() void Mission::update_offboard_mission() { + bool failed = true; + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ @@ -228,12 +230,15 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); + } + + if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 949231b159..8f1d6f8e85 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { + bool failed = false; /* Init if not done yet */ init(); @@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + // check if all mission item commands are supported + failed |= !checkMissionItemValidity(dm_current, nMissionItems); + if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + + return !failed; } bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { + // do not allow mission if we find unsupported item + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + // not supposed to happen unless the datamanager can't access the SD card, etc. + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card"); + return false; + } + + // check if we find unsupported item and reject mission if so + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_ROI && + missionitem.nav_cmd != NAV_CMD_PATHPLANNING && + missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); + return false; + } + } + mavlink_log_critical(_mavlink_fd, "Mission is valid!"); + return true; +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9a77a6dc25..94b6b29226 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,6 +62,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); From 5a53a4f7bfc5d4e40af6541efb2702209749eb21 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 21 May 2015 15:30:22 +0200 Subject: [PATCH 12/16] do not update mission if it has unsupported mission items --- src/modules/navigator/mission.cpp | 7 +++- .../navigator/mission_feasibility_checker.cpp | 42 ++++++++++++++++++- .../navigator/mission_feasibility_checker.h | 1 + 3 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c1763ba93d..a68f4de012 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -207,6 +207,8 @@ Mission::update_onboard_mission() void Mission::update_offboard_mission() { + bool failed = true; + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ @@ -228,12 +230,15 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); + } + + if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 949231b159..8f1d6f8e85 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { + bool failed = false; /* Init if not done yet */ init(); @@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + // check if all mission item commands are supported + failed |= !checkMissionItemValidity(dm_current, nMissionItems); + if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + + return !failed; } bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { + // do not allow mission if we find unsupported item + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + // not supposed to happen unless the datamanager can't access the SD card, etc. + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card"); + return false; + } + + // check if we find unsupported item and reject mission if so + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_ROI && + missionitem.nav_cmd != NAV_CMD_PATHPLANNING && + missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); + return false; + } + } + mavlink_log_critical(_mavlink_fd, "Mission is valid!"); + return true; +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9a77a6dc25..94b6b29226 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,6 +62,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); From 323759bb5279af62dcc4a0454d3ec06debae36ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 May 2015 22:06:54 +0200 Subject: [PATCH 13/16] commander: Fix error checking and handling of level routine --- src/modules/commander/accelerometer_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 32c0ffd8cc..f250eef475 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -557,7 +557,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; - const unsigned cal_hz = 250; + const unsigned cal_hz = 100; const unsigned settle_time = 30; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; @@ -612,6 +612,8 @@ int do_level_calibration(int mavlink_fd) { pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); + success = false; + goto out; } if (fabsf(roll_mean) > 0.8f ) { @@ -626,6 +628,7 @@ int do_level_calibration(int mavlink_fd) { success = true; } +out: if (success) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); return 0; From 2d22e8325d7c440131f40039f302603a25b10b94 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 13:45:53 -0700 Subject: [PATCH 14/16] Fix double build when using new make target syntax When make is invoked as "make posix posix_default" it will build the posix_default target twice. The Makefile was fixed to correct this. If "make posix" is run, the Makefile still calls "make PX4_TARGET_OS=posix". If "make posix posix_default" is run, the posix target just exports PX4_TARGET_OS=posix and then make evaluates the next goal (posix_default). Signed-off-by: Mark Charlebois --- Makefile | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index 3f78f32362..a14fd13a65 100644 --- a/Makefile +++ b/Makefile @@ -37,6 +37,7 @@ TARGETS := nuttx posix qurt EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) ifneq ($(EXPLICIT_TARGET),) export PX4_TARGET_OS=$(EXPLICIT_TARGET) + export GOALS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) endif # @@ -277,14 +278,12 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -nuttx: - make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) - -posix: - make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) - -qurt: - make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) +nuttx posix qurt: +ifeq ($(GOALS),) + make PX4_TARGET_OS=$@ $(GOALS) +else + export PX4_TARGET_OS=$@ +endif posixrun: Tools/posix_run.sh From f1f562a94d8697ab9aaaaa7d30645bfecb133602 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 15:35:11 -0700 Subject: [PATCH 15/16] POSIX: Fixed orb_advertise failure There were an insufficient number of devmap entries allocated and when they ran out, new orb_advertise requests would fail. Also added a new logging macro for the Linux build to show the calling pthread if enabled. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 13 ++++++++----- src/modules/simulator/simulator.cpp | 3 +++ src/platforms/posix/px4_layer/work_thread.c | 5 +++-- src/platforms/px4_log.h | 11 ++++++++--- 4 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 58db85169e..8ff8ae1fc8 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -64,7 +64,7 @@ private: px4_dev_t() {} }; -#define PX4_MAX_DEV 30 +#define PX4_MAX_DEV 50 static px4_dev_t *devmap[PX4_MAX_DEV]; /* @@ -99,7 +99,7 @@ VDev::~VDev() int VDev::register_class_devname(const char *class_devname) { - PX4_DEBUG("VDev::register_class_devname"); + PX4_DEBUG("VDev::register_class_devname %s", class_devname); if (class_devname == nullptr) { return -EINVAL; } @@ -124,7 +124,7 @@ VDev::register_class_devname(const char *class_devname) int VDev::register_driver(const char *name, void *data) { - PX4_DEBUG("VDev::register_driver"); + PX4_DEBUG("VDev::register_driver %s", name); int ret = -ENOSPC; if (name == NULL || data == NULL) @@ -145,14 +145,17 @@ VDev::register_driver(const char *name, void *data) break; } } + if (ret != PX4_OK) { + PX4_ERR("No free devmap entries - increase PX4_MAX_DEV"); + } return ret; } int VDev::unregister_driver(const char *name) { - PX4_DEBUG("VDev::unregister_driver"); - int ret = -ENOSPC; + PX4_DEBUG("VDev::unregister_driver %s", name); + int ret = -EINVAL; if (name == NULL) return -EINVAL; diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 83e3fd8d76..f51439faff 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -63,6 +63,9 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { + // Reads are paced from reading gyrosim and if + // we don't delay here we read too fast + usleep(50000); return _mpu.copyData(buf, len); } diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index 1128a80944..a21584cd0a 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -138,10 +138,11 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id) work_unlock(lock_id); if (!worker) { - printf("MESSED UP: worker = 0\n"); + PX4_ERR("MESSED UP: worker = 0"); } - else + else { worker(arg); + } /* Now, unfortunately, since we re-enabled interrupts we don't * know the state of the work list and we will have to start diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 07438a4ec4..a46d4e790b 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -50,19 +50,24 @@ printf(__VA_ARGS__);\ printf(" (file %s line %d)\n", __FILE__, __LINE__);\ } - #if defined(__PX4_QURT) #include -#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); +#define PX4_DEBUG(...) __px4_log_verbose("DEBUG", __VA_ARGS__); #define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); #define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); #define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); #elif defined(__PX4_LINUX) #include +#include + +#define __px4_log_threads(level, ...) { \ + printf("%-5s %ld ", level, pthread_self());\ + printf(__VA_ARGS__);\ + printf(" (file %s line %d)\n", __FILE__, __LINE__);\ +} -//#define PX4_DEBUG(...) { } #define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); #define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); #define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); From 6ae6c29879e2ede60013f4e3c43ca3f1a8543532 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 May 2015 15:45:13 -0700 Subject: [PATCH 16/16] QuRT: Changed PX4_DEBUG output from verbose to omit Signed-off-by: Mark Charlebois --- 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 a46d4e790b..a0d7f07bce 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -53,7 +53,7 @@ #if defined(__PX4_QURT) #include -#define PX4_DEBUG(...) __px4_log_verbose("DEBUG", __VA_ARGS__); +#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); #define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); #define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); #define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__);