From 8d15da379b966e89d983f58e390f6e03b622ed0a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 15 Oct 2018 13:33:32 -0700 Subject: [PATCH 0001/1046] nxphlite-v3:Add HolyBro GPS on I2C 2 using ist8310 --- ROMFS/px4fmu_common/init.d/rc.sensors | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 9712b1d7250f..1b8168502182 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -127,6 +127,9 @@ then # External I2C bus hmc5883 -C -X start + # Possible external compasses + ist8310 -C -b 2 start + # External I2C bus lis3mdl -X start From 246b879aeae14806ff383896040b067b2e3454c7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 10 Sep 2018 12:53:26 -0400 Subject: [PATCH 0002/1046] require MODULE_NAME --- cmake/common/px4_base.cmake | 3 +++ platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp | 2 +- platforms/posix/src/main.cpp | 1 + platforms/posix/src/px4_layer/CMakeLists.txt | 1 + src/drivers/samv7/drv_hrt.c | 4 ++-- src/lib/mixer/mixer_load.c | 6 ++---- src/platforms/apps.cpp.in | 2 ++ src/platforms/common/work_queue/CMakeLists.txt | 1 + src/platforms/px4_log.h | 5 ----- 9 files changed, 13 insertions(+), 12 deletions(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 8ae9d2cf9a24..60703be25ab4 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -212,7 +212,10 @@ function(px4_add_module) if(INCLUDES) target_include_directories(${MODULE}_original PRIVATE ${INCLUDES}) endif() + target_compile_definitions(${MODULE}_original PRIVATE PX4_MAIN=${MAIN}_app_main) + target_compile_definitions(${MODULE}_original PRIVATE MODULE_NAME="${MAIN}_original") + # unity build add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp COMMAND cat ${SRCS} > ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp DEPENDS ${MODULE}_original ${DEPENDS} ${SRCS} diff --git a/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp b/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp index 7495b030cce9..e760cee5e9b9 100644 --- a/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp +++ b/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp @@ -46,7 +46,7 @@ namespace px4 void init(int argc, char *argv[], const char *process_name) { - PX4_WARN("process: %s", process_name); + printf("process: %s\n", process_name); } uint64_t get_time_micros() diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp index f7d301392bcc..91739014cb67 100644 --- a/platforms/posix/src/main.cpp +++ b/platforms/posix/src/main.cpp @@ -75,6 +75,7 @@ #include "px4_daemon/server.h" #include "px4_daemon/pxh.h" +#define MODULE_NAME "px4" static const char *LOCK_FILE_PATH = "/tmp/px4_lock"; diff --git a/platforms/posix/src/px4_layer/CMakeLists.txt b/platforms/posix/src/px4_layer/CMakeLists.txt index f31a645dbac8..661da197fbde 100644 --- a/platforms/posix/src/px4_layer/CMakeLists.txt +++ b/platforms/posix/src/px4_layer/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(px4_layer drv_hrt.c ${SHMEM_SRCS} ) +target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_link_libraries(px4_layer PRIVATE work_queue) target_link_libraries(px4_layer PRIVATE px4_daemon) diff --git a/src/drivers/samv7/drv_hrt.c b/src/drivers/samv7/drv_hrt.c index 9774dac6fa93..cc6f94dd11df 100644 --- a/src/drivers/samv7/drv_hrt.c +++ b/src/drivers/samv7/drv_hrt.c @@ -347,7 +347,7 @@ hrt_tim_init(void) ret = sam_tc_clockselect(frequency, &cmr, &actual); if (ret < 0) { - PX4_PANIC("ERROR: Failed no divisor can be found (%d),for timer channel %d\n", ret, HRT_TIMER_CHANNEL); + printf("ERROR: Failed no divisor can be found (%d),for timer channel %d\n", ret, HRT_TIMER_CHANNEL); return; } @@ -386,7 +386,7 @@ hrt_tim_init(void) hrt_tch = sam_tc_allocate(HRT_TIMER_CHANNEL, cmr); if (!hrt_tch) { - PX4_PANIC("ERROR: Failed to allocate timer channel %d\n", HRT_TIMER_CHANNEL); + printf("ERROR: Failed to allocate timer channel %d\n", HRT_TIMER_CHANNEL); return; } diff --git a/src/lib/mixer/mixer_load.c b/src/lib/mixer/mixer_load.c index 8ca4b798750d..11454852e512 100644 --- a/src/lib/mixer/mixer_load.c +++ b/src/lib/mixer/mixer_load.c @@ -37,11 +37,9 @@ * Programmable multi-channel mixer library. */ -#include #include #include #include -#include #include "mixer_load.h" @@ -54,7 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) fp = fopen(fname, "r"); if (fp == NULL) { - warnx("file not found"); + printf("file not found\n"); return -1; } @@ -100,7 +98,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - warnx("line too long"); + printf("line too long\n"); fclose(fp); return -1; } diff --git a/src/platforms/apps.cpp.in b/src/platforms/apps.cpp.in index bc417688f450..123a138c4eec 100644 --- a/src/platforms/apps.cpp.in +++ b/src/platforms/apps.cpp.in @@ -10,6 +10,8 @@ #include +#define MODULE_NAME "px4" + extern "C" { ${builtin_apps_decl_string} diff --git a/src/platforms/common/work_queue/CMakeLists.txt b/src/platforms/common/work_queue/CMakeLists.txt index 7026bd6b453e..22eef4576141 100644 --- a/src/platforms/common/work_queue/CMakeLists.txt +++ b/src/platforms/common/work_queue/CMakeLists.txt @@ -50,6 +50,7 @@ if (NOT "${OS}" MATCHES "nuttx") work_queue.c work_thread.c ) + target_compile_definitions(work_queue PRIVATE MODULE_NAME="work_queue") add_dependencies(work_queue prebuild_targets) endif() diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 2a4a0d988ec6..1e6e2208b29f 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -168,11 +168,6 @@ __END_DECLS #define __px4__log_printcond(cond, ...) if (cond) printf(__VA_ARGS__) #define __px4__log_printline(level, ...) printf(__VA_ARGS__) - -#ifndef MODULE_NAME -#define MODULE_NAME "Unknown" -#endif - #define __px4__log_timestamp_fmt "%-10" PRIu64 " " #define __px4__log_timestamp_arg ,hrt_absolute_time() #define __px4__log_level_fmt "%-5s " From 4e2bf27da6f0dbb88ac968bcf2621f29eb4f14a1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Nov 2018 14:03:24 -0500 Subject: [PATCH 0003/1046] FlightTaskAuto set WaypointType from position setpoint types --- .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index f90a8a3f831b..7c7705804dbf 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -53,14 +53,14 @@ * of the size */ enum class WaypointType : int { - position = 0, - velocity, - loiter, - takeoff, - land, - idle, - offboard, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks - follow_target + position = position_setpoint_s::SETPOINT_TYPE_POSITION, + velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY, + loiter = position_setpoint_s::SETPOINT_TYPE_LOITER, + takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF, + land = position_setpoint_s::SETPOINT_TYPE_LAND, + idle = position_setpoint_s::SETPOINT_TYPE_IDLE, + offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks + follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET, }; enum class State { From f590137525fe924541f6b2b4f541ae72550b7b4d Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 12 Nov 2018 15:41:19 +0100 Subject: [PATCH 0004/1046] Fix HIL Revert "pwm_out_sim lazily publish actuator_outputs" This reverts commit bcad940a9f529388781433f30d432eb1f25a9feb. --- src/drivers/pwm_out_sim/PWMSim.cpp | 6 ++++-- src/drivers/pwm_out_sim/PWMSim.hpp | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 9a93dfbf8313..ef7054b3800e 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -158,6 +158,9 @@ PWMSim::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + /* advertise the mixed control outputs, insist on the first group output */ + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs); + update_params(); int params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -284,8 +287,7 @@ PWMSim::run() /* and publish for anyone that cares to see */ _actuator_outputs.timestamp = hrt_absolute_time(); - int instance; - orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &_actuator_outputs, &instance, ORB_PRIO_DEFAULT); + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs); // use first valid timestamp_sample for latency tracking for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 87c7beb61234..46772adf67e9 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -110,7 +110,7 @@ class PWMSim : public cdev::CDev, public ModuleBase int _armed_sub{-1}; - actuator_outputs_s _actuator_outputs{}; + actuator_outputs_s _actuator_outputs = {}; orb_advert_t _outputs_pub{nullptr}; unsigned _num_outputs{0}; From e7e06dfe38322e1fe9b7062293b8160e133b8a88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 28 Oct 2018 16:48:05 +0100 Subject: [PATCH 0005/1046] fix mc_pos_control: disable flight tasks if none of them should be running Previously when switching e.g. from stabilized from acro, the stabilized flight task kept running and publishing setpoints. Luckily it caused no problems, but the log showed arbitrary attitude setpoints. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 948562d82f40..ba80c8745107 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -775,7 +775,7 @@ MulticopterPositionControl::run() local_pos_sp.vz = _control.getVelSp()(2); thr_sp.copyTo(local_pos_sp.thrust); - // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller). + // Publish local position setpoint (for logging only) publish_local_pos_sp(local_pos_sp); _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller @@ -831,6 +831,7 @@ void MulticopterPositionControl::start_flight_task() { bool task_failure = false; + bool should_disable_task = true; int prev_failure_count = _task_failure_count; if (!_vehicle_status.is_rotary_wing) { @@ -839,6 +840,7 @@ MulticopterPositionControl::start_flight_task() } if (_vehicle_status.in_transition_mode) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::Transition); if (error != 0) { @@ -862,6 +864,7 @@ MulticopterPositionControl::start_flight_task() _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled)) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); if (error != 0) { @@ -879,6 +882,7 @@ MulticopterPositionControl::start_flight_task() // Auto-follow me if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); if (error != 0) { @@ -894,7 +898,8 @@ MulticopterPositionControl::start_flight_task() } } else if (_control_mode.flag_control_auto_enabled) { - // Auto relate maneuvers + // Auto related maneuvers + should_disable_task = false; int error = 0; switch (MPC_AUTO_MODE.get()) { case 0: @@ -928,6 +933,7 @@ MulticopterPositionControl::start_flight_task() // manual position control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { + should_disable_task = false; int error = 0; switch (MPC_POS_MODE.get()) { @@ -967,6 +973,7 @@ MulticopterPositionControl::start_flight_task() // manual altitude control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); if (error != 0) { @@ -985,6 +992,7 @@ MulticopterPositionControl::start_flight_task() // manual stabilized control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized); if (error != 0) { @@ -1011,6 +1019,8 @@ MulticopterPositionControl::start_flight_task() // No task was activated. _flight_tasks.switchTask(FlightTaskIndex::None); } + } else if (should_disable_task) { + _flight_tasks.switchTask(FlightTaskIndex::None); } } From 1946f043f1a2fc3e01f6e53ddb1c59655be08bda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 28 Oct 2018 16:48:53 +0100 Subject: [PATCH 0006/1046] mc_pos_control: print active flight task for 'mc_pos_control status' cmd --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ba80c8745107..c220a0959dcd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -95,6 +95,9 @@ class MulticopterPositionControl : public ModuleBase /** @see ModuleBase::run() */ void run() override; + /** @see ModuleBase::print_status() */ + int print_status() override; + private: bool _in_smooth_takeoff = false; /** Date: Mon, 5 Nov 2018 10:27:00 +0100 Subject: [PATCH 0007/1046] mc_pos_control: simplify weathervane logic --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c220a0959dcd..abc734da8705 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -633,8 +633,8 @@ MulticopterPositionControl::run() if (_wv_controller != nullptr) { // in manual mode we just want to use weathervane if position is controlled as well - if (_wv_controller->weathervane_enabled() && !(_control_mode.flag_control_manual_enabled - && !_control_mode.flag_control_position_enabled)) { + if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled + || _control_mode.flag_control_position_enabled)) { _wv_controller->activate(); } else { From f31f63fff7f5ad24e341d3ba7523c4482f4b212c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Nov 2018 10:31:20 +0100 Subject: [PATCH 0008/1046] mc_att_control: use orb_publish_auto use nullptr for instance to make sure we always get the first instance. --- .../mc_att_control/mc_att_control_main.cpp | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7c6a82047b75..85e4cbe45000 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -586,14 +586,7 @@ MulticopterAttitudeControl::publish_rates_setpoint() _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - + orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); } void @@ -607,9 +600,7 @@ MulticopterAttitudeControl::publish_rate_controller_status() rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); - - int instance; - orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, nullptr, ORB_PRIO_DEFAULT); } void @@ -631,13 +622,7 @@ MulticopterAttitudeControl::publish_actuator_controls() } if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub != nullptr) { - - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - - } else if (_actuators_id) { - _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); - } + orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); } } From ca7acae904b8361351194f287f9d36aef51101e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Nov 2018 10:40:46 +0100 Subject: [PATCH 0009/1046] mc stabilized mode: move from mc_pos_control to mc_att_control - better in terms of dependencies: - the position controller code depended on position topics for yaw - mc_pos_control does not have to be run for Stabilized mode - the code path is much simpler, and thus less error prone. This is especially important since Stabilized is often used as a fail-safe flight mode. --- src/lib/FlightTasks/CMakeLists.txt | 1 - .../tasks/ManualAltitude/CMakeLists.txt | 2 +- .../FlightTaskManualAltitude.cpp | 67 ++++++- .../FlightTaskManualAltitude.hpp | 58 ++++-- .../FlightTaskManualPosition.cpp | 3 +- .../tasks/ManualStabilized/CMakeLists.txt | 39 ---- .../FlightTaskManualStabilized.cpp | 172 ------------------ .../FlightTaskManualStabilized.hpp | 83 --------- src/modules/mc_att_control/mc_att_control.hpp | 29 ++- .../mc_att_control/mc_att_control_main.cpp | 169 +++++++++++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 19 -- 11 files changed, 297 insertions(+), 345 deletions(-) delete mode 100644 src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt delete mode 100644 src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp delete mode 100644 src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 62339338bacf..42e0c70f90d1 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -51,7 +51,6 @@ endif() # add core flight tasks to list list(APPEND flight_tasks_all - ManualStabilized ManualAltitude ManualAltitudeSmooth ManualPosition diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt index e3d9d8213295..14f9ce498624 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude FlightTaskManualAltitude.cpp ) -target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManualStabilized) +target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual) target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 0409451dd9c3..962fa0e625bb 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -43,19 +43,23 @@ using namespace matrix; bool FlightTaskManualAltitude::updateInitialize() { - bool ret = FlightTaskManualStabilized::updateInitialize(); - // in addition to stabilized require valid position and velocity in D-direction - return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)); + bool ret = FlightTaskManual::updateInitialize(); + // in addition to manual require valid position and velocity in D-direction and valid yaw + return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } bool FlightTaskManualAltitude::activate() { - bool ret = FlightTaskManualStabilized::activate(); - _thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity + bool ret = FlightTaskManual::activate(); + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.0f; + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.0f; _setDefaultConstraints(); + _constraints.tilt = math::radians(MPC_MAN_TILT_MAX.get()); + if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) { _constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; @@ -78,8 +82,9 @@ bool FlightTaskManualAltitude::activate() void FlightTaskManualAltitude::_scaleSticks() { - // reuse same scaling as for stabilized - FlightTaskManualStabilized::_scaleSticks(); + /* Scale sticks to yaw and thrust using + * linear scale for yaw and piecewise linear map for thrust. */ + _yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get()); // scale horizontal velocity with expo curve stick input const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; @@ -249,9 +254,47 @@ void FlightTaskManualAltitude::_respectMaxAltitude() } } +void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) +{ + float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); + v(0) = v_r(0); + v(1) = v_r(1); +} + +void FlightTaskManualAltitude::_updateHeadingSetpoints() +{ + /* Yaw-lock depends on stick input. If not locked, + * yaw_sp is set to NAN. + * TODO: add yawspeed to get threshold.*/ + if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { + // no fixed heading when rotating around yaw by stick + _yaw_setpoint = NAN; + + } else { + // hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + + } else { + // check reset counter and update yaw setpoint if necessary + if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) { + _yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); + _heading_reset_counter = _sub_attitude->get().quat_reset_counter; + } + } + } + + // check if an external yaw handler is active and if yes, let it compute the yaw setpoints + if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate(); + } +} + void FlightTaskManualAltitude::_updateSetpoints() { - FlightTaskManualStabilized::_updateHeadingSetpoints(); // get yaw setpoint + _updateHeadingSetpoints(); // get yaw setpoint // Thrust in xy are extracted directly from stick inputs. A magnitude of // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no @@ -271,3 +314,11 @@ void FlightTaskManualAltitude::_updateSetpoints() _updateAltitudeLock(); } + +bool FlightTaskManualAltitude::update() +{ + _scaleSticks(); + _updateSetpoints(); + + return true; +} diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index f7bb7a51d07e..b40dd1df3895 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -39,19 +39,31 @@ #pragma once -#include "FlightTaskManualStabilized.hpp" +#include "FlightTaskManual.hpp" -class FlightTaskManualAltitude : public FlightTaskManualStabilized +class FlightTaskManualAltitude : public FlightTaskManual { public: FlightTaskManualAltitude() = default; virtual ~FlightTaskManualAltitude() = default; bool activate() override; bool updateInitialize() override; + bool update() override; + + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *ext_yaw_handler) override { _ext_yaw_handler = ext_yaw_handler; } protected: - void _updateSetpoints() override; /**< updates all setpoints */ - void _scaleSticks() override; /**< scales sticks to velocity in z */ + void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ + virtual void _updateSetpoints(); /**< updates all setpoints */ + virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + + /** + * rotates vector into local frame + */ + void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /** * Check and sets for position lock. @@ -60,27 +72,15 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized */ void _updateAltitudeLock(); - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized, + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, (ParamFloat) MPC_HOLD_MAX_Z, (ParamInt) MPC_ALT_MODE, (ParamFloat) MPC_HOLD_MAX_XY, - (ParamFloat) MPC_Z_P + (ParamFloat) MPC_Z_P, + (ParamFloat) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */ + (ParamFloat) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */ ) private: - uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - float _max_speed_up = 10.0f; - float _min_speed_down = 1.0f; - bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ - - /** - * Distance to ground during terrain following. - * If user does not demand velocity change in D-direction and the vehcile - * is in terrain-following mode, then height to ground will be locked to - * _dist_to_ground_lock. - */ - float _dist_to_ground_lock = NAN; - /** * Terrain following. * During terrain following, the position setpoint is adjusted @@ -99,4 +99,22 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized void _respectMinAltitude(); void _respectMaxAltitude(); + + + WeatherVane *_ext_yaw_handler = + nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ + float _max_speed_up = 10.0f; + float _min_speed_down = 1.0f; + bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + + /** + * Distance to ground during terrain following. + * If user does not demand velocity change in D-direction and the vehcile + * is in terrain-following mode, then height to ground will be locked to + * _dist_to_ground_lock. + */ + float _dist_to_ground_lock = NAN; }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 9e98b93fc0e7..44c05ec4c357 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -53,10 +53,11 @@ bool FlightTaskManualPosition::updateInitialize() bool FlightTaskManualPosition::activate() { - // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(); + _constraints.tilt = math::radians(MPC_TILTMAX_AIR.get()); + // set task specific constraint if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) { _constraints.speed_xy = MPC_VEL_MANUAL.get(); diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt deleted file mode 100644 index 1e12883dbeb9..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(FlightTaskManualStabilized - FlightTaskManualStabilized.cpp -) - -target_link_libraries(FlightTaskManualStabilized PUBLIC FlightTaskManual) -target_include_directories(FlightTaskManualStabilized PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp deleted file mode 100644 index 239823d8a4c6..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightManualStabilized.cpp - */ - -#include "FlightTaskManualStabilized.hpp" -#include -#include - -using namespace matrix; - -bool FlightTaskManualStabilized::activate() -{ - bool ret = FlightTaskManual::activate(); - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get()); - _yaw_setpoint = _yaw; - _yawspeed_setpoint = 0.0f; - _constraints.tilt = math::radians(_tilt_max_man.get()); - return ret; -} - -bool FlightTaskManualStabilized::updateInitialize() -{ - bool ret = FlightTaskManual::updateInitialize(); - // need a valid yaw-state - return ret && PX4_ISFINITE(_yaw); -} - -void FlightTaskManualStabilized::_scaleSticks() -{ - /* Scale sticks to yaw and thrust using - * linear scale for yaw and piecewise linear map for thrust. */ - _yawspeed_setpoint = _sticks_expo(3) * math::radians(_yaw_rate_scaling.get()); - _throttle = _throttleCurve(); -} - -void FlightTaskManualStabilized::_updateHeadingSetpoints() -{ - /* Yaw-lock depends on stick input. If not locked, - * yaw_sp is set to NAN. - * TODO: add yawspeed to get threshold.*/ - if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; - - } else { - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - - } else { - // check reset counter and update yaw setpoint if necessary - if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) { - _yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); - _heading_reset_counter = _sub_attitude->get().quat_reset_counter; - } - } - } - - // check if an external yaw handler is active and if yes, let it compute the yaw setpoints - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { - _yaw_setpoint = NAN; - _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate(); - } -} - -void FlightTaskManualStabilized::_updateThrustSetpoints() -{ - /* Rotate setpoint into local frame. */ - Vector2f sp(&_sticks(0)); - _rotateIntoHeadingFrame(sp); - - /* Ensure that maximum tilt is in [0.001, Pi] */ - float tilt_max = math::constrain(_constraints.tilt, 0.001f, M_PI_F); - - const float x = sp(0) * tilt_max; - const float y = sp(1) * tilt_max; - - /* The norm of the xy stick input provides the pointing - * direction of the horizontal desired thrust setpoint. The magnitude of the - * xy stick inputs represents the desired tilt. Both tilt and magnitude can - * be captured through Axis-Angle. - */ - /* The Axis-Angle is the perpendicular vector to xy-stick input */ - Vector2f v = Vector2f(y, -x); - float v_norm = v.norm(); // the norm of v defines the tilt angle - - if (v_norm > tilt_max) { // limit to the configured maximum tilt angle - v *= tilt_max / v_norm; - } - - /* The final thrust setpoint is found by rotating the scaled unit vector pointing - * upward by the Axis-Angle. - * Make sure that the attitude can be controlled even at 0 throttle. - */ - Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f); - _thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * math::max(_throttle, 0.0001f); -} - -void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v) -{ - float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; - Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); - v(0) = v_r(0); - v(1) = v_r(1); -} - -void FlightTaskManualStabilized::_updateSetpoints() -{ - _updateHeadingSetpoints(); - _updateThrustSetpoints(); -} - -float FlightTaskManualStabilized::_throttleCurve() -{ - // Scale stick z from [-1,1] to [min thrust, max thrust] - float throttle = -((_sticks(2) - 1.0f) * 0.5f); - - switch (_throttle_curve.get()) { - case 1: // no rescaling - return throttle; - - default: // 0 or other: rescale to hover throttle at 0.5 stick - if (throttle < 0.5f) { - return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get(); - - } else { - return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get(); - } - } - -} - -bool FlightTaskManualStabilized::update() -{ - _scaleSticks(); - _updateSetpoints(); - - return true; -} diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp deleted file mode 100644 index 60b9247f3462..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightManualStabilized.hpp - * - * Flight task for manual controlled attitude. - * It generates thrust and yaw setpoints. - */ - -#pragma once - -#include "FlightTaskManual.hpp" - -class FlightTaskManualStabilized : public FlightTaskManual -{ -public: - FlightTaskManualStabilized() = default; - - virtual ~FlightTaskManualStabilized() = default; - bool activate() override; - bool updateInitialize() override; - bool update() override; - - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} - -protected: - virtual void _updateSetpoints(); /**< updates all setpoints */ - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ - virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */ - void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */ - -private: - void _updateThrustSetpoints(); /**< sets thrust setpoint */ - float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */ - - float _throttle{}; /** mapped from stick z */ - - WeatherVane *_ext_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, - (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _tilt_max_man, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _throttle_min_stabilized, /**< minimum throttle for stabilized */ - (ParamFloat) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/ - (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ - (ParamInt) _throttle_curve /**< throttle curve behavior */ - ) -}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ae9b06caf7a2..eb95da8d183f 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -114,6 +114,20 @@ class MulticopterAttitudeControl : public ModuleBase void publish_rates_setpoint(); void publish_rate_controller_status(); + float throttle_curve(float throttle_stick_input); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(float dt, bool reset_yaw_sp); + + /** + * Get the landing gear state based on the manual control switch position + * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN + */ + float get_landing_gear_state(); + + /** * Attitude controller. */ @@ -150,6 +164,7 @@ class MulticopterAttitudeControl : public ModuleBase orb_advert_t _v_rates_sp_pub{nullptr}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ + orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -186,6 +201,9 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + DEFINE_PARAMETERS( (ParamFloat) _roll_p, (ParamFloat) _roll_rate_p, @@ -221,6 +239,7 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _pitch_rate_max, (ParamFloat) _yaw_rate_max, (ParamFloat) _yaw_auto_max, + (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ (ParamFloat) _acro_roll_max, (ParamFloat) _acro_pitch_max, @@ -238,7 +257,14 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _board_offset_x, (ParamFloat) _board_offset_y, - (ParamFloat) _board_offset_z + (ParamFloat) _board_offset_z, + + /* Stabilized mode params */ + (ParamFloat) _man_tilt_max_deg, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _man_throttle_min, /**< minimum throttle for stabilized */ + (ParamFloat) _throttle_max, /**< maximum throttle for stabilized */ + (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ + (ParamInt) _throttle_curve /**< throttle curve behavior */ ) matrix::Vector3f _attitude_p; /**< P gain for attitude control */ @@ -251,6 +277,7 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _mc_rate_max; /**< attitude rate limits in stabilized modes */ matrix::Vector3f _auto_rate_max; /**< attitude rate limits in auto modes */ matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 85e4cbe45000..dd8a54555c05 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -177,6 +177,8 @@ MulticopterAttitudeControl::parameters_updated() _acro_rate_max(1) = math::radians(_acro_pitch_max.get()); _acro_rate_max(2) = math::radians(_acro_yaw_max.get()); + _man_tilt_max = math::radians(_man_tilt_max_deg.get()); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* get transformation matrix from sensor/board to body frame */ @@ -319,7 +321,15 @@ MulticopterAttitudeControl::vehicle_attitude_poll() orb_check(_v_att_sub, &updated); if (updated) { + uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + + // Check for a heading reset + if (prev_quat_reset_counter != _v_att.quat_reset_counter) { + // we only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + } return true; } return false; @@ -368,6 +378,143 @@ MulticopterAttitudeControl::vehicle_land_detected_poll() } +float +MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) +{ + // throttle_stick_input is in range [0, 1] + switch (_throttle_curve.get()) { + case 1: // no rescaling to hover throttle + return _man_throttle_min.get() + throttle_stick_input * (_throttle_max.get() - _man_throttle_min.get()); + + default: // 0 or other: rescale to hover throttle at 0.5 stick + if (throttle_stick_input < 0.5f) { + return (_throttle_hover.get() - _man_throttle_min.get()) / 0.5f * throttle_stick_input + _man_throttle_min.get(); + + } else { + return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _throttle_max.get(); + } + } +} + +float +MulticopterAttitudeControl::get_landing_gear_state() +{ + // Only switch the landing gear up if we are not landed and if + // the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (_vehicle_land_detected.landed) { + _gear_state_initialized = false; + } + float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down + if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; + + } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + // Switching the gear off does put it into a safe defined state + _gear_state_initialized = true; + } + + return landing_gear; +} + +void +MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(Quatf(_v_att.q)).psi(); + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else if (_manual_control_sp.z > 0.05f) { + + const float yaw_rate = math::radians(_yaw_rate_scaling.get()); + attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate; + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(x*x + y*y) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + const float x = _manual_control_sp.x * _man_tilt_max; + const float y = _manual_control_sp.y * _man_tilt_max; + + // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane + Vector2f v = Vector2f(y, -x); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); + Eulerf euler_sp = q_sp_rpy; + attitude_setpoint.roll_body = euler_sp(0); + attitude_setpoint.pitch_body = euler_sp(1); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); + + /* modify roll/pitch only if we're a VTOL */ + if (_vehicle_status.is_vtol) { + // Construct attitude setpoint rotation matrix. Modify the setpoints for roll + // and pitch such that they reflect the user's intention even if a large yaw error + // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix + // from the pure euler angle setpoints will lead to unexpected attitude behaviour from + // the user's view as the euler angle sequence uses the yaw setpoint and not the current + // heading of the vehicle. + // However there's also a coupling effect that causes oscillations for fast roll/pitch changes + // at higher tilt angles, so we want to avoid using this on multicopters. + // The effect of that can be seen with: + // - roll/pitch into one direction, keep it fixed (at high angle) + // - apply a fast yaw rotation + // - look at the roll and pitch angles: they should stay pretty much the same as when not yawing + + // calculate our current yaw error + float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw); + + // compute the vector obtained by rotating a z unit vector by the rotation + // given by the roll and pitch commands of the user + Vector3f zB = {0.0f, 0.0f, 1.0f}; + Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f); + Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB; + + // transform the vector into a new frame which is rotated around the z axis + // by the current yaw error. this vector defines the desired tilt when we look + // into the direction of the desired heading + Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); + z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + + // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] + // R_tilt is computed from_euler; only true if cos(roll) not equal zero + // -> valid if roll is not +-pi/2; + attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1)); + attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); + } + + /* copy quaternion setpoint to attitude setpoint topic */ + Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); + q_sp.copyTo(attitude_setpoint.q_d); + attitude_setpoint.q_d_valid = true; + + attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z); + + attitude_setpoint.landing_gear = get_landing_gear_state(); + attitude_setpoint.timestamp = hrt_absolute_time(); + orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); +} + /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) @@ -666,6 +813,9 @@ MulticopterAttitudeControl::run() float dt_accumulator = 0.f; int loop_counter = 0; + bool reset_yaw_sp = true; + float attitude_dt = 0.f; + while (!should_exit()) { poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; @@ -723,6 +873,7 @@ MulticopterAttitudeControl::run() vehicle_land_detected_poll(); const bool manual_control_updated = vehicle_manual_poll(); const bool attitude_updated = vehicle_attitude_poll(); + attitude_dt += dt; /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't @@ -734,8 +885,19 @@ MulticopterAttitudeControl::run() } } + bool attitude_setpoint_generated = false; + if (_v_control_mode.flag_control_attitude_enabled) { if (attitude_updated) { + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(attitude_dt, reset_yaw_sp); + attitude_setpoint_generated = true; + } + control_attitude(); publish_rates_setpoint(); } @@ -775,6 +937,13 @@ MulticopterAttitudeControl::run() } } + if (attitude_updated) { + reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || + _vehicle_land_detected.landed || + (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode + attitude_dt = 0.f; + } + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { dt_accumulator += dt; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index abc734da8705..a37da94612bf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1003,25 +1003,6 @@ MulticopterPositionControl::start_flight_task() } } - // manual stabilized control - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { - should_disable_task = false; - int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized); - - if (error != 0) { - if (prev_failure_count == 0) { - PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error)); - } - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_STAB); - task_failure = false; - } - } - // check task failure if (task_failure) { From 36ee0297ee5ab67c6f4429d5acf73b258321dfa2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Nov 2018 10:41:06 +0100 Subject: [PATCH 0010/1046] FlightTask.hpp: minor code cleanup --- src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 759a0b70d3b3..9efe587ee47d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -150,7 +150,7 @@ class FlightTask : public ModuleParams * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy. * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method. */ - virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}; + virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } @@ -166,7 +166,7 @@ class FlightTask : public ModuleParams */ void _resetSetpoints(); - /* + /** * Check and update local position */ void _evaluateVehicleLocalPosition(); From adf1fea554cf7c5f2231c0cd0400b920220ab286 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 5 Nov 2018 10:43:32 +0100 Subject: [PATCH 0011/1046] mc_att_control: fix for Rattitude mode When switching back from rate to attitude control, the code depended on a vehicle_control_mode topics update, but the publication frequency of that is low. So the switch was noticeably delayed. --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index dd8a54555c05..c4858a5db364 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -876,13 +876,12 @@ MulticopterAttitudeControl::run() attitude_dt += dt; /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't + * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { - if (fabsf(_manual_control_sp.y) > _rattitude_thres.get() || - fabsf(_manual_control_sp.x) > _rattitude_thres.get()) { - _v_control_mode.flag_control_attitude_enabled = false; - } + _v_control_mode.flag_control_attitude_enabled = + fabsf(_manual_control_sp.y) <= _rattitude_thres.get() && + fabsf(_manual_control_sp.x) <= _rattitude_thres.get(); } bool attitude_setpoint_generated = false; From c9e52d43862b6854d4396b2190cb4b9f1b957c0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Nov 2018 08:20:51 +0100 Subject: [PATCH 0012/1046] MPC_MAN_TILT_MAX: increase max limit from 90 to 180 degrees This is especially useful for testing (the vehicle must behave correctly even at high tilt angles). --- src/modules/mc_pos_control/mc_pos_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 7fdd1b55b29b..10cebefa9f3c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -334,7 +334,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * @unit deg * @min 0.0 - * @max 90.0 + * @max 180.0 * @decimal 1 * @group Multicopter Position Control */ @@ -568,7 +568,7 @@ PARAM_DEFINE_FLOAT(MPC_JERK_MIN, 8.0f); PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); /** - * Manual control stick exponential curve sensitivity attenuation with small velocity setpoints + * Manual position control stick exponential curve sensitivity * * The higher the value the less sensitivity the stick has around zero * while still reaching the maximum value with full stick deflection. From 2b7d3bd088c7cba8187a33e0cde2737555f8accc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Nov 2018 08:23:58 +0100 Subject: [PATCH 0013/1046] flight tasks: move weather vane yaw handler from Altitude to Position task --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 6 ------ .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp | 8 -------- .../tasks/ManualPosition/FlightTaskManualPosition.cpp | 7 +++++++ .../tasks/ManualPosition/FlightTaskManualPosition.hpp | 9 +++++++++ 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 962fa0e625bb..2b422b5247e9 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -284,12 +284,6 @@ void FlightTaskManualAltitude::_updateHeadingSetpoints() } } } - - // check if an external yaw handler is active and if yes, let it compute the yaw setpoints - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { - _yaw_setpoint = NAN; - _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate(); - } } void FlightTaskManualAltitude::_updateSetpoints() diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index b40dd1df3895..491a17c215f9 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -50,11 +50,6 @@ class FlightTaskManualAltitude : public FlightTaskManual bool updateInitialize() override; bool update() override; - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) override { _ext_yaw_handler = ext_yaw_handler; } - protected: void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ virtual void _updateSetpoints(); /**< updates all setpoints */ @@ -101,9 +96,6 @@ class FlightTaskManualAltitude : public FlightTaskManual void _respectMaxAltitude(); - WeatherVane *_ext_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _max_speed_up = 10.0f; float _min_speed_down = 1.0f; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 44c05ec4c357..12bc4586dea8 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -147,6 +147,13 @@ void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateSetpoints() { FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints _updateXYlock(); // check for position lock } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 4b0b9ed46f51..04a2064fdfa1 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -51,6 +51,11 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude bool activate() override; bool updateInitialize() override; + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -65,4 +70,8 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude private: float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + + WeatherVane *_weathervane_yaw_handler = + nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + }; From 330941c1fc59929ad73a65a344f19e567eabf14e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Nov 2018 08:25:50 +0100 Subject: [PATCH 0014/1046] FlightTaskManualAltitude: initialze yaw setpoint with NAN Also update some comments. In case we activate the task and don't have a locked yaw, we should initialize the yaw setpoint with NAN to avoid any abrupt changes. --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 6 ++---- .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 2b422b5247e9..5cbc4636b781 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -51,7 +51,7 @@ bool FlightTaskManualAltitude::updateInitialize() bool FlightTaskManualAltitude::activate() { bool ret = FlightTaskManual::activate(); - _yaw_setpoint = _yaw; + _yaw_setpoint = NAN; _yawspeed_setpoint = 0.0f; _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); @@ -82,11 +82,9 @@ bool FlightTaskManualAltitude::activate() void FlightTaskManualAltitude::_scaleSticks() { - /* Scale sticks to yaw and thrust using - * linear scale for yaw and piecewise linear map for thrust. */ + // Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed _yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get()); - // scale horizontal velocity with expo curve stick input const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks_expo(2); } diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 491a17c215f9..474c0cce6b9f 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -71,7 +71,7 @@ class FlightTaskManualAltitude : public FlightTaskManual (ParamFloat) MPC_HOLD_MAX_Z, (ParamInt) MPC_ALT_MODE, (ParamFloat) MPC_HOLD_MAX_XY, - (ParamFloat) MPC_Z_P, + (ParamFloat) MPC_Z_P, /**< position controller altitude propotional gain */ (ParamFloat) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */ (ParamFloat) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */ ) From b3afa6ddac8632beff6207f189a368101daf5ef9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Nov 2018 08:26:19 +0100 Subject: [PATCH 0015/1046] mc_pos_control: fix task activation failure for Transition --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a37da94612bf..0b50c5d7b4c2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -848,6 +848,7 @@ MulticopterPositionControl::start_flight_task() bool should_disable_task = true; int prev_failure_count = _task_failure_count; + // Do not run any flight task for VTOLs in fixed-wing mode if (!_vehicle_status.is_rotary_wing) { _flight_tasks.switchTask(FlightTaskIndex::None); return; @@ -858,7 +859,9 @@ MulticopterPositionControl::start_flight_task() int error = _flight_tasks.switchTask(FlightTaskIndex::Transition); if (error != 0) { - PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); + if (prev_failure_count == 0) { + PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error)); + } task_failure = true; _task_failure_count++; From 9a9c17b9792ddba0cc0cc1bf22987423aa0d15b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 9 Nov 2018 08:26:50 +0100 Subject: [PATCH 0016/1046] vehicle_attitude_setpoint: add comment that attitude sp angles can be NaN for FW --- msg/vehicle_attitude_setpoint.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index b3b27874af7a..da0b01b97f24 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -2,9 +2,9 @@ uint64 timestamp # time since system start (microseconds) int8 LANDING_GEAR_UP = 1 # landing gear up int8 LANDING_GEAR_DOWN = -1 # landing gear down -float32 roll_body # body angle in NED frame -float32 pitch_body # body angle in NED frame -float32 yaw_body # body angle in NED frame +float32 roll_body # body angle in NED frame (can be NaN for FW) +float32 pitch_body # body angle in NED frame (can be NaN for FW) +float32 yaw_body # body angle in NED frame (can be NaN for FW) float32 yaw_sp_move_rate # rad/s (commanded by user) From 9379c010f1577377142e1be0d90864f4b6971b2c Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Mon, 12 Nov 2018 14:44:20 +1100 Subject: [PATCH 0017/1046] TRAJECTORY message renamed --- msg/vehicle_trajectory_waypoint.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/vehicle_trajectory_waypoint.msg b/msg/vehicle_trajectory_waypoint.msg index f30160a3737f..182f548d23a6 100644 --- a/msg/vehicle_trajectory_waypoint.msg +++ b/msg/vehicle_trajectory_waypoint.msg @@ -1,4 +1,4 @@ -# Vehicle Waypoints Trajectory description. See also Mavlink TRAJECTORY msg +# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg # The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. # The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. From 9594ebf72efdfff1a88aaf2206a90836ee9b9130 Mon Sep 17 00:00:00 2001 From: Mara Bos Date: Tue, 13 Nov 2018 20:31:22 +0100 Subject: [PATCH 0018/1046] Use unix sockets instead of pipes for posix daemon. (#10766) Unlike pipes, unix sockets provide bi-directional communication with each connected client. - No need to generate a unique uuid per client anymore. - The client doesn't have to create its own pipe anymore. - Since there is no risk of multiple client's writes getting mixed up, messages don't need to fit in a single write anymore, removing the limit on command length. - Since the server can detect a connection closing, the client no longer needs to handle signals. When the client is killed, the connection is automatically closed, which will cause the server to kill the related px4 thread. Since this does not rely on handling signals and the client sending an additional message, this is much more reliable. - Client is no longer a singleton. - The protocol is simplified. Standard output is directly written to the socket back to the client, without wrapping it in any protocol message. - Because of the simple protocol, one could now even use netcat to run a px4 command: $ echo hello | netcat -UN /tmp/px4-sock-0 Also removes a few race conditions. --- platforms/posix/src/main.cpp | 2 - platforms/posix/src/px4_daemon/CMakeLists.txt | 2 +- platforms/posix/src/px4_daemon/client.cpp | 314 +++++----------- platforms/posix/src/px4_daemon/client.h | 46 +-- platforms/posix/src/px4_daemon/server.cpp | 337 +++++++----------- platforms/posix/src/px4_daemon/server.h | 48 +-- platforms/posix/src/px4_daemon/server_io.cpp | 30 +- .../{pipe_protocol.cpp => sock_protocol.cpp} | 38 +- .../{pipe_protocol.h => sock_protocol.h} | 59 +-- 9 files changed, 254 insertions(+), 622 deletions(-) rename platforms/posix/src/px4_daemon/{pipe_protocol.cpp => sock_protocol.cpp} (62%) rename platforms/posix/src/px4_daemon/{pipe_protocol.h => sock_protocol.h} (52%) diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp index 91739014cb67..7cf1cae21ab8 100644 --- a/platforms/posix/src/main.cpp +++ b/platforms/posix/src/main.cpp @@ -173,8 +173,6 @@ int main(int argc, char **argv) argv[0] += path_length + strlen(prefix); px4_daemon::Client client(instance); - client.generate_uuid(); - client.register_sig_handler(); return client.process_args(argc, (const char **)argv); } else { diff --git a/platforms/posix/src/px4_daemon/CMakeLists.txt b/platforms/posix/src/px4_daemon/CMakeLists.txt index 9704af02c199..e6cf9fcc7524 100644 --- a/platforms/posix/src/px4_daemon/CMakeLists.txt +++ b/platforms/posix/src/px4_daemon/CMakeLists.txt @@ -37,6 +37,6 @@ px4_add_library(px4_daemon client.cpp server.cpp server_io.cpp - pipe_protocol.cpp + sock_protocol.cpp ) diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4_daemon/client.cpp index 5f76cf1a6b2b..194875aabe73 100644 --- a/platforms/posix/src/px4_daemon/client.cpp +++ b/platforms/posix/src/px4_daemon/client.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,13 +35,15 @@ * * @author Julian Oes * @author Beat Küng + * @author Mara Bos */ #include #include #include -#include +#include #include +#include #include #include @@ -53,60 +55,33 @@ namespace px4_daemon { - -namespace client -{ -static Client *_instance; -} - Client::Client(int instance_id) : - _uuid(0), - _client_send_pipe_fd(-1), + _fd(-1), _instance_id(instance_id) -{ - client::_instance = this; -} - -Client::~Client() -{ - client::_instance = nullptr; -} +{} int -Client::generate_uuid() +Client::process_args(const int argc, const char **argv) { - int rand_fd = open("/dev/urandom", O_RDONLY); - - if (rand_fd < 0) { - PX4_ERR("open urandom"); - return rand_fd; - } - - int ret = 0; + std::string sock_path = get_socket_path(_instance_id); - int rand_read = read(rand_fd, &_uuid, sizeof(_uuid)); + _fd = socket(AF_UNIX, SOCK_STREAM, 0); - if (rand_read != sizeof(_uuid)) { - PX4_ERR("rand read fail"); - ret = -errno; + if (_fd < 0) { + PX4_ERR("error creating socket"); + return -1; } - close(rand_fd); - return ret; -} - -int -Client::process_args(const int argc, const char **argv) -{ - // Prepare return pipe first to avoid a race. - int ret = _prepare_recv_pipe(); + sockaddr_un addr = {}; + addr.sun_family = AF_UNIX; + strncpy(addr.sun_path, sock_path.c_str(), sizeof(addr.sun_path) - 1); - if (ret != 0) { - PX4_ERR("Could not prepare recv pipe"); - return -2; + if (connect(_fd, (sockaddr *)&addr, sizeof(addr)) < 0) { + PX4_ERR("error connecting to socket"); + return -1; } - ret = _send_cmds(argc, argv); + int ret = _send_cmds(argc, argv); if (ret != 0) { PX4_ERR("Could not send commands"); @@ -116,70 +91,43 @@ Client::process_args(const int argc, const char **argv) return _listen(); } - -int -Client::_prepare_recv_pipe() -{ - int ret = get_client_recv_pipe_path(_uuid, _recv_pipe_path, sizeof(_recv_pipe_path)); - - if (ret < 0) { - PX4_ERR("failed to assemble path"); - return ret; - } - - ret = mkfifo(_recv_pipe_path, 0666); - - if (ret < 0) { - PX4_ERR("pipe %s already exists, errno: %d, %s", _recv_pipe_path, errno, strerror(errno)); - return ret; - } - - return 0; -} - int Client::_send_cmds(const int argc, const char **argv) { - // Send the command to server. - client_send_packet_s packet; - packet.header.msg_id = client_send_packet_s::message_header_s::e_msg_id::EXECUTE; - packet.header.client_uuid = _uuid; - packet.payload.execute_msg.is_atty = isatty(STDOUT_FILENO); - - // Concat arguments to send them. std::string cmd_buf; for (int i = 0; i < argc; ++i) { cmd_buf += argv[i]; if (i + 1 != argc) { + // TODO: Use '\0' as argument separator (and parse this server-side as well), + // so (quoted) whitespace within arguments doesn't get lost. cmd_buf += " "; } } - if (cmd_buf.size() >= sizeof(packet.payload.execute_msg.cmd)) { - PX4_ERR("commmand too long"); - return -1; - } + // Last byte is 'isatty'. + cmd_buf.push_back(isatty(STDOUT_FILENO)); - strcpy((char *)packet.payload.execute_msg.cmd, cmd_buf.c_str()); + size_t n = cmd_buf.size(); + const char *buf = cmd_buf.data(); - // The size is +1 because we want to include the null termination. - packet.header.payload_length = cmd_buf.size() + 1 + sizeof(packet.payload.execute_msg.is_atty); + while (n > 0) { + int n_sent = write(_fd, buf, n); - _client_send_pipe_fd = open(get_client_send_pipe_path(_instance_id).c_str(), O_WRONLY); + if (n_sent < 0) { + PX4_ERR("write() failed: %s", strerror(errno)); + return -1; + } - if (_client_send_pipe_fd < 0) { - PX4_ERR("pipe open fail (%i)", errno); - return _client_send_pipe_fd; + n -= n_sent; + buf += n_sent; } - int bytes_to_send = get_client_send_packet_length(&packet); - int bytes_sent = write(_client_send_pipe_fd, &packet, bytes_to_send); - - if (bytes_sent != bytes_to_send) { - PX4_ERR("write fail (%i)", errno); - return bytes_sent; + // Let the server know we're done writing. + if (shutdown(_fd, SHUT_WR) < 0) { + PX4_ERR("shutdown() failed: %s", strerror(errno)); + return -1; } return 0; @@ -188,159 +136,57 @@ Client::_send_cmds(const int argc, const char **argv) int Client::_listen() { - int client_recv_pipe_fd = open(_recv_pipe_path, O_RDONLY); - - if (client_recv_pipe_fd < 0) { - PX4_ERR("open failed, errno: %d, %s", errno, strerror(errno)); - } - - bool exit_loop = false; - int exit_arg = 0; - - while (!exit_loop) { - - // We only read as much as we need, otherwise we might get out of - // sync with packets. - client_recv_packet_s packet_recv; - int bytes_read = read(client_recv_pipe_fd, &packet_recv, sizeof(client_recv_packet_s::header)); - - if (bytes_read > 0) { - - // Using the header we can determine how big the payload is. - int payload_to_read = sizeof(packet_recv) - - sizeof(packet_recv.header) - - sizeof(packet_recv.payload) - + packet_recv.header.payload_length; - - // Again, we only read as much as we need because otherwise we need - // hold a buffer and parse it. - bytes_read = read(client_recv_pipe_fd, ((uint8_t *)&packet_recv) + bytes_read, payload_to_read); - - if (bytes_read > 0) { - - int retval = 0; - bool should_exit = false; - - int parse_ret = _parse_client_recv_packet(packet_recv, retval, should_exit); - - if (parse_ret != 0) { - PX4_ERR("retval could not be parsed"); - exit_arg = -1; - - } else { - exit_arg = retval; - } - - exit_loop = should_exit; - - } else if (bytes_read == 0) { - exit_arg = 0; - exit_loop = true; + char buffer[1024]; + int n_buffer_used = 0; + + // The response ends in {0, retval}. So when we detect a 0, or a 0 followed + // by another byte, we don't output it yet, until we know whether it was + // the end of the stream or not. + while (true) { + int n_read = read(_fd, buffer + n_buffer_used, sizeof buffer - n_buffer_used); + + if (n_read < 0) { + PX4_ERR("unable to read from socket"); + return -1; + + } else if (n_read == 0) { + if (n_buffer_used == 2) { + return buffer[1]; + + } else { + // Missing return value at end of stream. Stream was abruptly ended. + return -1; } - } else if (bytes_read == 0) { - // 0 means the pipe has been closed by all clients. - exit_arg = 0; - exit_loop = true; + } else { + n_read += n_buffer_used; + + if (n_read >= 2 && buffer[n_read - 2] == 0) { + // If the buffer ends in {0, retval}, keep it. + fwrite(buffer, n_read - 2, 1, stdout); + buffer[0] = 0; + buffer[1] = buffer[n_read - 1]; + n_buffer_used = 2; + + } else if (n_read >= 1 && buffer[n_read - 1] == 0) { + // If the buffer ends in a 0-byte, keep it. + fwrite(buffer, n_read - 1, 1, stdout); + buffer[0] = 0; + n_buffer_used = 1; + + } else { + fwrite(buffer, n_read, 1, stdout); + n_buffer_used = 0; + } } } - - close(_client_send_pipe_fd); - return exit_arg; -} - -int -Client::_parse_client_recv_packet(const client_recv_packet_s &packet, int &retval, bool &should_exit) -{ - switch (packet.header.msg_id) { - case client_recv_packet_s::message_header_s::e_msg_id::RETVAL: - - should_exit = true; - return _retval_cmd_packet(packet, retval); - - case client_recv_packet_s::message_header_s::e_msg_id::STDOUT: - - should_exit = false; - return _stdout_msg_packet(packet); - - default: - should_exit = true; - PX4_ERR("recv msg_id not handled: %d", (int)packet.header.msg_id); - return -1; - } } -int -Client::_retval_cmd_packet(const client_recv_packet_s &packet, int &retval) -{ - if (packet.header.payload_length == sizeof(packet.payload.retval_msg.retval)) { - retval = packet.payload.retval_msg.retval; - return 0; - - } else { - PX4_ERR("payload size wrong"); - return -1; - } -} - -int -Client::_stdout_msg_packet(const client_recv_packet_s &packet) -{ - if (packet.header.payload_length <= sizeof(packet.payload.stdout_msg.text)) { - - printf("%s", packet.payload.stdout_msg.text); - - return 0; - - } else { - PX4_ERR("payload size wrong (%i > %zu)", packet.header.payload_length, sizeof(packet.payload.stdout_msg.text)); - return -1; - } -} - -void -Client::register_sig_handler() -{ - // Register handlers for Ctrl+C to kill the thread if something hangs. - struct sigaction sig_int {}; - sig_int.sa_handler = Client::_static_sig_handler; - - // Without the flag SA_RESTART, we can't use open() after Ctrl+C has - // been pressed, and we can't wait for the return value from the - // cancelled command. - sig_int.sa_flags = SA_RESTART; - sigaction(SIGINT, &sig_int, nullptr); - sigaction(SIGTERM, &sig_int, nullptr); -} - -void -Client::_static_sig_handler(int sig_num) -{ - client::_instance->_sig_handler(sig_num); -} - -void -Client::_sig_handler(int sig_num) +Client::~Client() { - client_send_packet_s packet; - packet.header.msg_id = client_send_packet_s::message_header_s::e_msg_id::KILL; - packet.header.client_uuid = _uuid; - packet.payload.kill_msg.cmd_id = sig_num; - packet.header.payload_length = sizeof(packet.payload.kill_msg.cmd_id); - - if (_client_send_pipe_fd < 0) { - PX4_ERR("pipe open fail"); - system_exit(-1); - } - - int bytes_to_send = get_client_send_packet_length(&packet); - int bytes_sent = write(_client_send_pipe_fd, &packet, bytes_to_send); - - if (bytes_sent != bytes_to_send) { - PX4_ERR("write fail"); - system_exit(-1); + if (_fd >= 0) { + close(_fd); } } } // namespace px4_daemon - diff --git a/platforms/posix/src/px4_daemon/client.h b/platforms/posix/src/px4_daemon/client.h index 2e454c4cc25e..41f4cb838192 100644 --- a/platforms/posix/src/px4_daemon/client.h +++ b/platforms/posix/src/px4_daemon/client.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,21 +33,22 @@ /** * @file client.h * - * The client can write a command to the pipe that is supplied by the server. - * It will then open another pipe with its own UUID encoded and listen for - * stdout of the process that it started and the return value. + * The client can connect and write a command to the socket that is supplied by + * the server. It will then close its half of the connection, and read back the + * stdout stream of the process that it started, followed by its return value. * - * It the client receives a signal (e.g. Ctrl+C) it will catch it and send it - * as a message to the server in order to terminate the thread. + * It the client dies, the connection gets closed automatically and the corresponding + * thread in the server gets cancelled. * * @author Julian Oes * @author Beat Küng + * @author Mara Bos */ #pragma once #include -#include "pipe_protocol.h" +#include "sock_protocol.h" namespace px4_daemon { @@ -57,19 +58,14 @@ class Client { public: Client(int instance_id = 0); + ~Client(); - /** - * Initialize the unique ID of the client. - * - * @return 0 on success. - */ - int generate_uuid(); - - /** - * Make sure to catch signals in order to forward them to the server. - */ - void register_sig_handler(); + Client(Client &&other) : _fd(other._fd), _instance_id(other._instance_id) + { + // Steal the fd from the moved-from client. + other._fd = -1; + } /** * Process the supplied command line arguments and send them to server. @@ -81,22 +77,10 @@ class Client int process_args(const int argc, const char **argv); private: - int _prepare_recv_pipe(); int _send_cmds(const int argc, const char **argv); int _listen(); - int _parse_client_recv_packet(const client_recv_packet_s &packet, int &retval, bool &should_exit); - - int _retval_cmd_packet(const client_recv_packet_s &packet, int &retval); - int _stdout_msg_packet(const client_recv_packet_s &packet); - - - static void _static_sig_handler(int sig_num); - void _sig_handler(int sig_num); - - uint64_t _uuid; - int _client_send_pipe_fd; - char _recv_pipe_path[RECV_PIPE_PATH_LEN]; + int _fd; int _instance_id; ///< instance ID for running multiple instances of the px4 server }; diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4_daemon/server.cpp index 83fd4a0caefe..f9f5b568547c 100644 --- a/platforms/posix/src/px4_daemon/server.cpp +++ b/platforms/posix/src/px4_daemon/server.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,29 +35,27 @@ * * @author Julian Oes * @author Beat Küng + * @author Mara Bos */ +#include #include #include +#include #include #include #include #include +#include #include +#include +#include #include #include "pxh.h" #include "server.h" -// In Cygwin opening and closing the same named pipe multiple times within one process doesn't work POSIX compliant. -// As a workaround we open the client send pipe file in read write mode such that we can keep it open all the time. -#if !defined(__PX4_CYGWIN) -#define CLIENT_SEND_PIPE_OFLAGS O_RDONLY -#else -#define CLIENT_SEND_PIPE_OFLAGS O_RDWR -#endif - namespace px4_daemon { @@ -78,22 +76,37 @@ Server::~Server() int Server::start() { - std::string client_send_pipe_path = get_client_send_pipe_path(_instance_id); + std::string sock_path = get_socket_path(_instance_id); + + // Delete socket in case it exists already. + unlink(sock_path.c_str()); + + _fd = socket(AF_UNIX, SOCK_STREAM, 0); + + if (_fd < 0) { + PX4_ERR("error creating socket"); + return -1; + } - // Delete pipe in case it exists already. - unlink(client_send_pipe_path.c_str()); + sockaddr_un addr = {}; + addr.sun_family = AF_UNIX; + strncpy(addr.sun_path, sock_path.c_str(), sizeof(addr.sun_path) - 1); - // Create new pipe to listen to clients. - // This needs to happen before we return from this method, so that the caller can launch clients. - mkfifo(client_send_pipe_path.c_str(), 0666); + if (bind(_fd, (sockaddr *)&addr, sizeof(addr)) < 0) { + PX4_ERR("error binding socket"); + return -1; + } + + if (listen(_fd, 10) < 0) { + PX4_ERR("error listing to socket"); + return -1; + } if (0 != pthread_create(&_server_main_pthread, nullptr, _server_main_trampoline, - nullptr)) { - + this)) { PX4_ERR("error creating client handler thread"); - return -1; } @@ -101,12 +114,9 @@ Server::start() } void * -Server::_server_main_trampoline(void *arg) +Server::_server_main_trampoline(void *self) { - if (_instance) { - _instance->_server_main(arg); - } - + ((Server *)self)->_server_main(); return nullptr; } @@ -116,9 +126,8 @@ void Server::_pthread_key_destructor(void *arg) } void -Server::_server_main(void *arg) +Server::_server_main() { - // Set thread specific pipe to supplied file descriptor. int ret = pthread_key_create(&_key, _pthread_key_destructor); if (ret != 0) { @@ -126,241 +135,151 @@ Server::_server_main(void *arg) return; } - std::string client_send_pipe_path = get_client_send_pipe_path(_instance_id); - int client_send_pipe_fd = open(client_send_pipe_path.c_str(), CLIENT_SEND_PIPE_OFLAGS); + // The list of file descriptors to watch. + std::vector poll_fds; - while (true) { + // Watch the listening socket for incoming connections. + poll_fds.push_back(pollfd {_fd, POLLIN, 0}); - client_send_packet_s packet; + while (true) { + int n_ready = poll(poll_fds.data(), poll_fds.size(), -1); - // We only read as much as we need, otherwise we might get out of - // sync with packets. - int bytes_read = read(client_send_pipe_fd, &packet, sizeof(client_send_packet_s::header)); + if (n_ready < 0) { + PX4_ERR("poll() failed: %s", strerror(errno)); + return; + } - if (bytes_read > 0) { + _lock(); - // Using the header we can determine how big the payload is. - int payload_to_read = sizeof(packet) - - sizeof(packet.header) - - sizeof(packet.payload) - + packet.header.payload_length; + // Handle any new connections. + if (poll_fds[0].revents & POLLIN) { + --n_ready; + int client = accept(_fd, nullptr, nullptr); - // Again, we only read as much as we need because otherwise we need - // hold a buffer and parse it. - bytes_read = read(client_send_pipe_fd, ((uint8_t *)&packet) + bytes_read, payload_to_read); + if (client == -1) { + PX4_ERR("failed to accept client: %s", strerror(errno)); + _unlock(); + return; + } - if (bytes_read > 0) { + // Start a new thread to handle the client. + pthread_t *thread = &_fd_to_thread[client]; + ret = pthread_create(thread, nullptr, Server::_handle_client, (void *)(intptr_t)client); - _parse_client_send_packet(packet); + if (ret != 0) { + PX4_ERR("could not start pthread (%i)", ret); + _fd_to_thread.erase(client); + close(client); + } else { + // We won't join the thread, so detach to automatically release resources at its end + pthread_detach(*thread); } - } - if (bytes_read == 0) { - // 0 means the pipe has been closed by all clients - // and we need to re-open it. - close(client_send_pipe_fd); - client_send_pipe_fd = open(client_send_pipe_path.c_str(), O_RDONLY); + // Start listening for the client hanging up. + poll_fds.push_back(pollfd {client, POLLHUP, 0}); } - } - close(client_send_pipe_fd); -} + // Handle any closed connections. + for (size_t i = 1; n_ready > 0 && i < poll_fds.size();) { + if (poll_fds[i].revents) { + --n_ready; + auto thread = _fd_to_thread.find(poll_fds[i].fd); -void -Server::_parse_client_send_packet(const client_send_packet_s &packet) -{ - switch (packet.header.msg_id) { - case client_send_packet_s::message_header_s::e_msg_id::EXECUTE: - _execute_cmd_packet(packet); - break; - - case client_send_packet_s::message_header_s::e_msg_id::KILL: - _kill_cmd_packet(packet); - break; - - default: - PX4_ERR("send msg_id not handled"); - break; - } -} + if (thread != _fd_to_thread.end()) { + // Thread is still running, so we cancel it. + // TODO: use a more graceful exit method to avoid resource leaks + pthread_cancel(thread->second); + _fd_to_thread.erase(thread); + } -void -Server::_execute_cmd_packet(const client_send_packet_s &packet) -{ - if (packet.header.payload_length == 0) { - PX4_ERR("command length 0"); - return; - } + close(poll_fds[i].fd); + poll_fds.erase(poll_fds.begin() + i); - // We open the client's specific pipe to write the return value and stdout back to. - // The pipe's path is created knowing the UUID of the client. - char path[RECV_PIPE_PATH_LEN]; - int ret = get_client_recv_pipe_path(packet.header.client_uuid, path, RECV_PIPE_PATH_LEN); - - if (ret < 0) { - PX4_ERR("failed to assemble path"); - return; - } - - int pipe_fd = open(path, O_WRONLY); - - if (pipe_fd < 0) { - PX4_ERR("pipe open fail"); - return; - } - - // To execute a command we start a new thread. - pthread_t new_pthread; - - // We need to copy everything that the new thread needs because we will go - // out of scope. - RunCmdArgs *args = new RunCmdArgs; - strncpy(args->cmd, (char *)packet.payload.execute_msg.cmd, sizeof(args->cmd)); - args->client_uuid = packet.header.client_uuid; - args->pipe_fd = pipe_fd; - args->is_atty = packet.payload.execute_msg.is_atty; - - _lock(); // need to lock, otherwise the thread could already exit before we insert into the map - ret = pthread_create(&new_pthread, nullptr, Server::_run_cmd, (void *)args); + } else { + ++i; + } + } - if (ret != 0) { - PX4_ERR("could not start pthread (%i)", ret); - delete args; - - } else { - // We won't join the thread, so detach to automatically release resources at its end - pthread_detach(new_pthread); - // We keep two maps for cleanup if a thread is finished or killed. - _client_uuid_to_pthread.insert(std::pair - (packet.header.client_uuid, new_pthread)); - _pthread_to_pipe_fd.insert(std::pair - (new_pthread, pipe_fd)); + _unlock(); } - _unlock(); + close(_fd); } - void -Server::_kill_cmd_packet(const client_send_packet_s &packet) +*Server::_handle_client(void *arg) { - _lock(); + int fd = (int)(intptr_t)arg; - // TODO: we currently ignore the signal type. - auto client_uuid_iter = _client_uuid_to_pthread.find(packet.header.client_uuid); + // Read until the end of the incoming stream. + std::string cmd; - if (client_uuid_iter == _client_uuid_to_pthread.end()) { - _unlock(); - return; - } - - pthread_t pthread_to_kill = client_uuid_iter->second; - - // TODO: use a more graceful exit method to avoid resource leaks - int ret = pthread_cancel(pthread_to_kill); + while (true) { + size_t n = cmd.size(); + cmd.resize(n + 1024); + ssize_t n_read = read(fd, &cmd[n], cmd.size() - n); - __cleanup_thread(packet.header.client_uuid); + if (n_read < 0) { + _cleanup(fd); + return nullptr; + } - _unlock(); + cmd.resize(n + n_read); - if (ret != 0) { - PX4_ERR("failed to cancel thread"); + if (n_read == 0) { + break; + } } - // We don't send retval when we get killed. - // The client knows this and just exits without confirmation. -} - - - -void -*Server::_run_cmd(void *arg) -{ - RunCmdArgs *args = (RunCmdArgs *)arg; - - // Copy arguments so that we can cleanup the arg structure. - uint64_t client_uuid = args->client_uuid; - int pipe_fd = args->pipe_fd; - bool is_atty = args->is_atty; - std::string message_str(args->cmd); + if (cmd.size() < 2) { + _cleanup(fd); + return nullptr; + } - // Clean up the args from the heap in case the thread gets canceled - // from outside. - delete args; + // Last byte is 'isatty'. + uint8_t isatty = cmd.back(); + cmd.pop_back(); // We register thread specific data. This is used for PX4_INFO (etc.) log calls. CmdThreadSpecificData *thread_data_ptr; if ((thread_data_ptr = (CmdThreadSpecificData *)pthread_getspecific(_instance->_key)) == nullptr) { thread_data_ptr = new CmdThreadSpecificData; - thread_data_ptr->pipe_fd = pipe_fd; - thread_data_ptr->is_atty = is_atty; - thread_data_ptr->packet.header.msg_id = client_recv_packet_s::message_header_s::e_msg_id::STDOUT; + thread_data_ptr->fd = fd; + thread_data_ptr->is_atty = isatty; (void)pthread_setspecific(_instance->_key, (void *)thread_data_ptr); } // Run the actual command. - int retval = Pxh::process_line(message_str, true); + int retval = Pxh::process_line(cmd, true); // Report return value. - _send_retval(pipe_fd, retval, client_uuid); - - // Clean up before returning. - _instance->_cleanup_thread(client_uuid); + char buf[2] = {0, (char)retval}; - return nullptr; -} - - -void -Server::_send_retval(const int pipe_fd, const int retval, const uint64_t client_uuid) -{ - client_recv_packet_s packet; - - packet.header.msg_id = client_recv_packet_s::message_header_s::e_msg_id::RETVAL; - packet.header.payload_length = sizeof(packet.payload.retval_msg); - packet.payload.retval_msg.retval = retval; - - int bytes_to_send = get_client_recv_packet_length(&packet); - int bytes_sent = write(pipe_fd, &packet, bytes_to_send); - - if (bytes_sent != bytes_to_send) { - printf("write fail\n"); - return; + if (write(fd, buf, sizeof buf) < 0) { + // Don't care it went wrong, as we're cleaning up anyway. } -} -void -Server::_cleanup_thread(const uint64_t client_uuid) -{ - _lock(); - __cleanup_thread(client_uuid); - _unlock(); + _cleanup(fd); + return nullptr; } void -Server::__cleanup_thread(const uint64_t client_uuid) +Server::_cleanup(int fd) { - pthread_t pthread_killed = _client_uuid_to_pthread[client_uuid]; - auto pipe_iter = _pthread_to_pipe_fd.find(pthread_killed); - - if (pipe_iter == _pthread_to_pipe_fd.end()) { - // can happen if the thread already exited and then got a kill packet - PX4_DEBUG("pipe fd already closed"); - return; - } - - int pipe_fd = pipe_iter->second; - close(pipe_fd); - - char path[RECV_PIPE_PATH_LEN] = {}; - get_client_recv_pipe_path(client_uuid, path, RECV_PIPE_PATH_LEN); - unlink(path); - - _client_uuid_to_pthread.erase(client_uuid); - _pthread_to_pipe_fd.erase(pthread_killed); + _instance->_lock(); + _instance->_fd_to_thread.erase(fd); + _instance->_unlock(); + + // We can't close() the fd here, since the main thread is probably + // polling for it: close()ing it causes a race condition. + // So, we only call shutdown(), which causes the main thread to register a + // 'POLLHUP', such that the main thread can close() it for us. + // We already removed this thread from _fd_to_thread, so there is no risk + // of the main thread trying to cancel this thread after it already exited. + shutdown(fd, SHUT_RDWR); } } //namespace px4_daemon diff --git a/platforms/posix/src/px4_daemon/server.h b/platforms/posix/src/px4_daemon/server.h index e1e0f8b84539..1ce8d0767595 100644 --- a/platforms/posix/src/px4_daemon/server.h +++ b/platforms/posix/src/px4_daemon/server.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016, 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,12 +33,11 @@ /** * @file server.h * - * The server (also called daemon) opens a pipe for clients to write to. + * The server (also called daemon) opens a socket for clients to connect to. * - * Once a client connects it will send a command as well as a unique ID. + * Once a client connects it will send a command and close its side of the connection. * The server will return the stdout of the executing command, as well as the return - * value to the client on a client specific pipe. - * The client specific pipe are idenified by the unique ID of the client. + * value to the client. * * There should only every be one server running, therefore the static instance. * The Singleton implementation is not complete, but it should be obvious not @@ -46,6 +45,7 @@ * * @author Julian Oes * @author Beat Küng + * @author Mara Bos */ #pragma once @@ -54,7 +54,7 @@ #include #include -#include "pipe_protocol.h" +#include "sock_protocol.h" namespace px4_daemon @@ -74,11 +74,10 @@ class Server */ int start(); - struct CmdThreadSpecificData { - int pipe_fd; // pipe fd to send data to descriptor + int fd; // fd to send stdout to bool is_atty; // whether file descriptor refers to a terminal - client_recv_packet_s packet; + char buffer[1024]; }; static bool is_running() @@ -92,17 +91,7 @@ class Server } private: static void *_server_main_trampoline(void *arg); - void _server_main(void *arg); - - void _parse_client_send_packet(const client_send_packet_s &packet); - void _execute_cmd_packet(const client_send_packet_s &packet); - void _kill_cmd_packet(const client_send_packet_s &packet); - void _cleanup_thread(const uint64_t client_uuid); - - /** - * Like _cleanup_thread(), but does not take the mutex - */ - void __cleanup_thread(const uint64_t client_uuid); + void _server_main(); void _lock() { @@ -114,28 +103,19 @@ class Server pthread_mutex_unlock(&_mutex); } - - static void _send_retval(const int pipe_fd, const int retval, const uint64_t client_uuid); - - struct RunCmdArgs { - char cmd[sizeof(client_send_packet_s::payload.execute_msg.cmd)]; - uint64_t client_uuid; - bool is_atty; - int pipe_fd; - }; - - static void *_run_cmd(void *arg); + static void *_handle_client(void *arg); + static void _cleanup(int fd); pthread_t _server_main_pthread; - std::map _pthread_to_pipe_fd; - std::map _client_uuid_to_pthread; - pthread_mutex_t _mutex; ///< protects access to _pthread_to_pipe_fd and _client_uuid_to_pthread + std::map _fd_to_thread; + pthread_mutex_t _mutex; ///< Protects _fd_to_thread. pthread_key_t _key; int _instance_id; ///< instance ID for running multiple instances of the px4 server + int _fd; static void _pthread_key_destructor(void *arg); diff --git a/platforms/posix/src/px4_daemon/server_io.cpp b/platforms/posix/src/px4_daemon/server_io.cpp index 72564b5c882d..ba801053da44 100644 --- a/platforms/posix/src/px4_daemon/server_io.cpp +++ b/platforms/posix/src/px4_daemon/server_io.cpp @@ -50,7 +50,7 @@ #include "server.h" #include -#include "pipe_protocol.h" +#include "sock_protocol.h" using namespace px4_daemon; @@ -84,10 +84,8 @@ int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty) #endif - client_recv_packet_s *packet = &thread_data_ptr->packet; - - *buffer = (char *)packet->payload.stdout_msg.text; - *max_length = sizeof(packet->payload.stdout_msg.text); + *buffer = thread_data_ptr->buffer; + *max_length = sizeof(thread_data_ptr->buffer); *is_atty = thread_data_ptr->is_atty; return 0; @@ -95,8 +93,6 @@ int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty) int send_stdout_pipe_buffer(unsigned buffer_length) { - assert(buffer_length <= sizeof(client_recv_packet_s::payload.stdout_msg.text)); - Server::CmdThreadSpecificData *thread_data_ptr; if (!Server::is_running()) { @@ -108,25 +104,9 @@ int send_stdout_pipe_buffer(unsigned buffer_length) return -1; } - client_recv_packet_s *packet = &thread_data_ptr->packet; - packet->header.payload_length = buffer_length; - - int pipe_fd = thread_data_ptr->pipe_fd; - int bytes_to_send = get_client_recv_packet_length(packet); - - // Check if we can write first by writing 0 bytes. - // If we don't do this, we'll get SIGPIPE and be very unhappy - // because the whole process will go down. - int ret = write(pipe_fd, nullptr, 0); - - if (ret == 0 && errno == EPIPE) { - printf("Error: can't write to closed pipe, giving up.\n"); - pthread_exit(nullptr); - } - - int bytes_sent = write(pipe_fd, packet, bytes_to_send); + int bytes_sent = write(thread_data_ptr->fd, thread_data_ptr->buffer, buffer_length); - if (bytes_sent != bytes_to_send) { + if (bytes_sent != (int)buffer_length) { printf("write fail\n"); return -1; } diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.cpp b/platforms/posix/src/px4_daemon/sock_protocol.cpp similarity index 62% rename from platforms/posix/src/px4_daemon/pipe_protocol.cpp rename to platforms/posix/src/px4_daemon/sock_protocol.cpp index 57bc95ebfb09..65b9e18000c4 100644 --- a/platforms/posix/src/px4_daemon/pipe_protocol.cpp +++ b/platforms/posix/src/px4_daemon/sock_protocol.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,44 +31,20 @@ * ****************************************************************************/ /** - * @file pipe_protocol.cpp + * @file sock_protocol.cpp * - * @author Julian Oes - * @author Beat Küng + * @author Mara Bos */ -#include -#include -#include - -#include "pipe_protocol.h" - -static const char CLIENT_SEND_PIPE_PATH[] = "/tmp/px4_client_send_pipe-"; -static const char CLIENT_RECV_PIPE_PATH[] = "/tmp/px4_client_recv_pipe"; - +#include "sock_protocol.h" namespace px4_daemon { - -unsigned get_client_send_packet_length(const client_send_packet_s *packet) -{ - return sizeof(client_send_packet_s) - sizeof(packet->payload) + packet->header.payload_length; -} - -unsigned get_client_recv_packet_length(const client_recv_packet_s *packet) -{ - return sizeof(client_recv_packet_s) - sizeof(packet->payload) + packet->header.payload_length; -} - -int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len) -{ - return snprintf(path, path_len, "%s-%016" PRIx64, CLIENT_RECV_PIPE_PATH, uuid); -} - -std::string get_client_send_pipe_path(int instance_id) +std::string get_socket_path(int instance_id) { - return std::string(CLIENT_SEND_PIPE_PATH) + std::to_string(instance_id); + // TODO: Use /var/run/px4/$instance/sock (or /var/run/user/$UID/... for non-root). + return "/tmp/px4-sock-" + std::to_string(instance_id); } } // namespace px4_daemon diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.h b/platforms/posix/src/px4_daemon/sock_protocol.h similarity index 52% rename from platforms/posix/src/px4_daemon/pipe_protocol.h rename to platforms/posix/src/px4_daemon/sock_protocol.h index 0f8012cdfd72..f158a29b66d0 100644 --- a/platforms/posix/src/px4_daemon/pipe_protocol.h +++ b/platforms/posix/src/px4_daemon/sock_protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,69 +31,18 @@ * ****************************************************************************/ /** - * @file pipe_protocol.h + * @file sock_protocol.h * - * @author Julian Oes - * @author Beat Küng + * @author Mara Bos */ #pragma once -#include #include namespace px4_daemon { -static const unsigned RECV_PIPE_PATH_LEN = 64; - -// The packet size is no more than 512 bytes, because that is the minimum guaranteed size -// for a pipe to avoid interleaving of messages when multiple clients write at the same time -// (atomic writes). -struct client_send_packet_s { - struct message_header_s { - uint64_t client_uuid; - enum class e_msg_id : int { - EXECUTE, - KILL - } msg_id; - unsigned payload_length; - } header; - - union { - struct execute_msg_s { - uint8_t is_atty; - uint8_t cmd[512 - sizeof(message_header_s) - sizeof(uint8_t)]; - } execute_msg; - struct kill_msg_s { - int cmd_id; - } kill_msg; - } payload; -}; - -// We have per client receiver a pipe with the uuid in its file path. -struct client_recv_packet_s { - struct message_header_s { - enum class e_msg_id : int { - RETVAL, - STDOUT - } msg_id; - unsigned payload_length; - } header; - - union { - struct retval_msg_s { - int retval; - } retval_msg; - struct stdout_msg_s { - uint8_t text[512 - sizeof(message_header_s)]; ///< null-terminated string (payload_length includes the null) - } stdout_msg; - } payload; -}; - -unsigned get_client_send_packet_length(const client_send_packet_s *packet); -unsigned get_client_recv_packet_length(const client_recv_packet_s *packet); -int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len); -std::string get_client_send_pipe_path(int instance_id); +std::string get_socket_path(int instance_id); } // namespace px4_daemon From d7c34ddee4b266bfe83dc8a5211b50b082501158 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 13 Nov 2018 20:32:40 +0100 Subject: [PATCH 0019/1046] mavlink: fix yawing to North for LOITER_TIME (#10828) * mavlink: yaw should wrap at 2 pi, not pi * mavlink: use newer rad/deg conversion --- src/modules/mavlink/mavlink_mission.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index fcecccc79aee..5a5b7c674bfe 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -53,7 +53,7 @@ #include #include -using matrix::wrap_pi; +using matrix::wrap_2pi; dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; bool MavlinkMissionManager::_dataman_init = false; @@ -1321,13 +1321,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->nav_cmd = NAV_CMD_WAYPOINT; mission_item->time_inside = mavlink_mission_item->param1; mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_UNLIM: mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; mission_item->loiter_radius = mavlink_mission_item->param3; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_TIME: @@ -1335,19 +1335,22 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->time_inside = mavlink_mission_item->param1; mission_item->loiter_radius = mavlink_mission_item->param3; mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0); + // Yaw is only valid for multicopter but we set it always because + // it's just ignored for fixedwing. + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LAND: mission_item->nav_cmd = NAV_CMD_LAND; // TODO: abort alt param1 - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); mission_item->land_precision = mavlink_mission_item->param2; break; case MAV_CMD_NAV_TAKEOFF: mission_item->nav_cmd = NAV_CMD_TAKEOFF; mission_item->pitch_min = mavlink_mission_item->param1; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_LOITER_TO_ALT: @@ -1383,7 +1386,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->yaw = wrap_pi(math::radians(mavlink_mission_item->param4)); + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; case MAV_CMD_NAV_FENCE_RETURN_POINT: @@ -1621,7 +1624,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param4 = math::degrees(mission_item->yaw); break; case MAV_CMD_NAV_FENCE_RETURN_POINT: From c882ca93892ea9759b0da58454a6da8af21bdb05 Mon Sep 17 00:00:00 2001 From: Mara Bos Date: Wed, 14 Nov 2018 17:04:17 +0100 Subject: [PATCH 0020/1046] On Posix: Don't shutdown (half) the connection from the client. This triggered POLLHUP too early on Mac. --- platforms/posix/src/px4_daemon/client.cpp | 6 ------ platforms/posix/src/px4_daemon/server.cpp | 5 +++-- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4_daemon/client.cpp index 194875aabe73..f6604b9566cb 100644 --- a/platforms/posix/src/px4_daemon/client.cpp +++ b/platforms/posix/src/px4_daemon/client.cpp @@ -124,12 +124,6 @@ Client::_send_cmds(const int argc, const char **argv) buf += n_sent; } - // Let the server know we're done writing. - if (shutdown(_fd, SHUT_WR) < 0) { - PX4_ERR("shutdown() failed: %s", strerror(errno)); - return -1; - } - return 0; } diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4_daemon/server.cpp index f9f5b568547c..d327385507cd 100644 --- a/platforms/posix/src/px4_daemon/server.cpp +++ b/platforms/posix/src/px4_daemon/server.cpp @@ -220,14 +220,15 @@ void cmd.resize(n + 1024); ssize_t n_read = read(fd, &cmd[n], cmd.size() - n); - if (n_read < 0) { + if (n_read <= 0) { _cleanup(fd); return nullptr; } cmd.resize(n + n_read); - if (n_read == 0) { + // Command ends in 0x00 (no tty) or 0x01 (tty). + if (!cmd.empty() && cmd.back() < 2) { break; } } From c2e036837045e8de778fbe097ed582f9297d5ec4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Nov 2018 20:43:53 -0500 Subject: [PATCH 0021/1046] Update submodule Mavlink v2.0 to latest Thu Nov 15 00:37:24 UTC 2018 - mavlink v2.0 in PX4/Firmware (0b3f4014bf751cde2a08abb578c10ae5606404b1): https://github.com/mavlink/c_library_v2/commit/32dcf73bb24516dc0d52119e624fda8987a03f62 - mavlink v2.0 current upstream: https://github.com/mavlink/c_library_v2/commit/408a7bedf3074bcae265ea0ce8ec5bb3519ed272 - Changes: https://github.com/mavlink/c_library_v2/compare/32dcf73bb24516dc0d52119e624fda8987a03f62...408a7bedf3074bcae265ea0ce8ec5bb3519ed272 408a7be 2018-11-13 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/f3e60ba1ca9c458364cc9538499a1335aa2feb5e c18232f 2018-11-12 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/2c52b06e125f88f6b674db112f127dc35d7a9c71 49a94c7 2018-11-09 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/c788ca3e143fbb0fd1f90a14fd2a473673b50973 0c4b296 2018-10-29 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/387c0fd71547d71f1e89578c8ef78c6bac8d6094 40bfa6c 2018-10-29 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/4590822ebde2a5737504c832669d74310afa4b8e ad2c30b 2018-10-26 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/38ecb35a3e3e6702b5522ac33188a53cc1fe698e 9fbf059 2018-10-24 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/30b63eb0c20160ddeeca3eef4238f370d3f3d43f 7915ad4 2018-10-23 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/2c6a8807b85317275c9d027afba7f646eb7db961 4371e84 2018-10-23 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/573e35231804ad775796472bcdcd331b4479cf4d d67e172 2018-10-22 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/40f83b2d4ff43fa65d08bb5ebeb9186dc2e8c2a6 852935c 2018-10-16 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/d683ddc28b1254ea8645dd2a18e5765b6bab0eba --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 32dcf73bb245..408a7bedf307 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 32dcf73bb24516dc0d52119e624fda8987a03f62 +Subproject commit 408a7bedf3074bcae265ea0ce8ec5bb3519ed272 From 88f62710618d61ae020822df6ab89644cef109a5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Nov 2018 13:54:51 -0500 Subject: [PATCH 0022/1046] Jenkins add tests on MacOS --- .ci/Jenkinsfile-compile | 1 + 1 file changed, 1 insertion(+) diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index f6dc68e41811..3f28381db437 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -31,6 +31,7 @@ pipeline { sh('ccache -z') sh('make posix_sitl_default') sh('ccache -s') + sh('make tests') } catch (exc) { throw (exc) From 0863e95a2cd0d208406a4c71173cf8394b847187 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Oct 2018 16:46:55 +0200 Subject: [PATCH 0023/1046] Add "Tracking Anti-Reset Windup" in velocity PID controller --- .../mc_pos_control/PositionControl.cpp | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index ba2b28f8e881..d31dd0f3f6bc 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -291,25 +291,17 @@ void PositionControl::_velocityController(const float &dt) _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE; } - // Get the direction of (r-y) in NE-direction. - float direction_NE = Vector2f(vel_err) * Vector2f(_vel_sp); + // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output + // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 + float arw_gain = 2.f / MPC_XY_VEL_P.get(); - // Apply Anti-Windup in NE-direction. - bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE && - direction_NE >= 0.0f); + Vector2f vel_err_lim; + vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain; + vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain; - if (!stop_integral_NE) { - _thr_int(0) += vel_err(0) * MPC_XY_VEL_I.get() * dt; - _thr_int(1) += vel_err(1) * MPC_XY_VEL_I.get() * dt; - - // magnitude of thrust integral can never exceed maximum throttle in NE - float integral_mag_NE = Vector2f(_thr_int).length(); - - if (integral_mag_NE > 0.0f && integral_mag_NE > thrust_max_NE) { - _thr_int(0) = _thr_int(0) / integral_mag_NE * thrust_max_NE; - _thr_int(1) = _thr_int(1) / integral_mag_NE * thrust_max_NE; - } - } + // Update integral + _thr_int(0) += MPC_XY_VEL_I.get() * vel_err_lim(0) * dt; + _thr_int(1) += MPC_XY_VEL_I.get() * vel_err_lim(1) * dt; } } From d852ce20e6504abc02e6aba38b9755540f5de101 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Oct 2018 18:04:30 +0200 Subject: [PATCH 0024/1046] MPC - Increase max velocity integral gain to 3.0 --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 10cebefa9f3c..28818fab2e76 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -217,7 +217,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.09f); * Non-zero value allows to resist wind. * * @min 0.0 - * @max 0.1 + * @max 3.0 * @decimal 3 * @group Multicopter Position Control */ From 7779c0bd6970720056e830dbff758a1fc50b08d0 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 15 Nov 2018 17:54:29 +0100 Subject: [PATCH 0025/1046] MPC - Update velocity integrator definition --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 28818fab2e76..4ac55f0decf5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -214,7 +214,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.09f); /** * Integral gain for horizontal velocity error * - * Non-zero value allows to resist wind. + * Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind. * * @min 0.0 * @max 3.0 From 4901f8a1dc5e35f6f7a4c9511dcfdaa425fd8ef1 Mon Sep 17 00:00:00 2001 From: dkang Date: Wed, 14 Nov 2018 17:00:51 +0900 Subject: [PATCH 0026/1046] add execption check after malloc --- platforms/posix/src/px4_layer/px4_posix_tasks.cpp | 5 +++++ platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp index 607d5071b922..ff3bf3aa49f0 100644 --- a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp +++ b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp @@ -144,6 +144,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // not safe to pass stack data to the thread creation pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len); + + if (taskdata == nullptr) { + return -ENOMEM; + } + memset(taskdata, 0, structsize + len); unsigned long offset = ((unsigned long)taskdata) + structsize; diff --git a/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp b/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp index 5d1093159d97..e6c8e191e8a9 100644 --- a/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp +++ b/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp @@ -147,6 +147,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize + len); + + if (taskdata == nullptr) { + return -ENOMEM; + } + offset = ((unsigned long)taskdata) + structsize; taskdata->entry = entry; From 2c111222cf564a86ce7014ff5107a21d1062c5ad Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 15 Nov 2018 15:35:28 +0000 Subject: [PATCH 0027/1046] logger: fix vision topics naming --- src/modules/logger/logger.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index a5f4c93e5199..ef6652e7bc74 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -653,13 +653,13 @@ void Logger::add_default_topics() add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_magnetometer", 200); + add_topic("vehicle_mocap_odometry"); add_topic("vehicle_rates_setpoint", 30); add_topic("vehicle_status", 200); add_topic("vehicle_status_flags"); add_topic("vehicle_trajectory_waypoint", 200); add_topic("vehicle_trajectory_waypoint_desired", 200); - add_topic("vehicle_vision_attitude"); - add_topic("vehicle_vision_position"); + add_topic("vehicle_visual_odometry"); add_topic("vtol_vehicle_status", 200); add_topic("wind_estimate", 200); @@ -720,8 +720,7 @@ void Logger::add_estimator_replay_topics() add_topic("vehicle_land_detected"); add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); - add_topic("vehicle_vision_attitude"); - add_topic("vehicle_vision_position"); + add_topic("vehicle_visual_odometry"); } void Logger::add_thermal_calibration_topics() From 05d9932f6c4fd1955b79033a27ab7edb3823b53f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 15 Nov 2018 19:34:36 +0000 Subject: [PATCH 0028/1046] logger: limit vehicle_*_odometry topics to 30hz on default logging --- src/modules/logger/logger.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ef6652e7bc74..bf92004165fd 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -653,13 +653,13 @@ void Logger::add_default_topics() add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_magnetometer", 200); - add_topic("vehicle_mocap_odometry"); + add_topic("vehicle_mocap_odometry", 30); add_topic("vehicle_rates_setpoint", 30); add_topic("vehicle_status", 200); add_topic("vehicle_status_flags"); add_topic("vehicle_trajectory_waypoint", 200); add_topic("vehicle_trajectory_waypoint_desired", 200); - add_topic("vehicle_visual_odometry"); + add_topic("vehicle_visual_odometry", 30); add_topic("vtol_vehicle_status", 200); add_topic("wind_estimate", 200); From 4278dd8fb43e46c36db7f6515ab5e3fa5a206836 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 09:34:48 -0500 Subject: [PATCH 0029/1046] posix px4_layer fix style --- platforms/posix/src/px4_layer/px4_posix_tasks.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp index ff3bf3aa49f0..bb428eae64fa 100644 --- a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp +++ b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp @@ -144,9 +144,9 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // not safe to pass stack data to the thread creation pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len); - + if (taskdata == nullptr) { - return -ENOMEM; + return -ENOMEM; } memset(taskdata, 0, structsize + len); From 30fe0798443f4cdc563b92516621b3dba318195f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 09:34:57 -0500 Subject: [PATCH 0030/1046] qurt px4_layer fix style --- platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp b/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp index e6c8e191e8a9..d0a719f67eb4 100644 --- a/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp +++ b/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp @@ -149,7 +149,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata = (pthdata_t *)malloc(structsize + len); if (taskdata == nullptr) { - return -ENOMEM; + return -ENOMEM; } offset = ((unsigned long)taskdata) + structsize; From 8090734fe1aefaadaa4963411f2bf8de281cdea5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 15 Nov 2018 17:18:23 +0100 Subject: [PATCH 0031/1046] appveyor: switch cache to the entire PX4 folder to save the time running the installer for every build which was necessary before because the folder exceedes the maximum of 1GB cache of the appveyor free plan --- appveyor.yml | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 5c36e87f05ba..73f093eef59b 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -19,10 +19,14 @@ init: - ver install: -- ps: if (-not (Test-Path C:\Toolchain.msi)) {Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.4.msi -OutFile C:\Toolchain.msi} -- start /wait msiexec /i C:\Toolchain.msi /quiet /qn /norestart /log C:\install.log - -# Note: using start /wait is important +# if the toolchain wasn't restored from build cache download and install it +- ps: >- + if (-not (Test-Path C:\PX4)) { + Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.4.msi -OutFile C:\Toolchain.msi + Start-Process -Wait msiexec -ArgumentList '/I C:\Toolchain.msi /quiet /qn /norestart /log C:\install.log' + } + +# Note: using Start-Process -Wait is important # because otherwise the install begins but non-blocking and the result cannot be used just after build_script: @@ -31,7 +35,7 @@ build_script: # safe the repopath for switching to it in cygwin bash - for /f %%i in ('cygpath -u %%CD%%') do set repopath=%%i # fetch all submodules in parallel -- call bash --login -c "cd $repopath && git submodule update --init --recursive --jobs=10" +- call bash --login -c "cd $repopath && git submodule -q update --init --recursive --jobs=10" # make SITL - call bash --login -c "cd $repopath && make posix" # make pixracer to check NuttX build @@ -41,6 +45,7 @@ build_script: # because otherwise certain things (like python; import numpy) do not work cache: -# cache toolchain installation file to avoid downloading it from AWS S3 each time -# it's ~496MB < 1GB free limit for build caches -- C:\Toolchain.msi -> appveyor.yml +# cache the entire toolchain installation folder to avoid +# downloading it from AWS S3 and installing the MSI each time +# it's ~1.8GB > 1GB free limit for build caches +- C:\PX4 -> appveyor.yml From 3ee596284d4d19226ac921696d0cb55835af975b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 15 Nov 2018 07:43:48 +1100 Subject: [PATCH 0032/1046] ecl: Magnetic declination use improvements Fixes bugs causing learned or GEO lookup declination to not be used in some circumstances. https://github.com/PX4/ecl/pull/518 https://github.com/PX4/ecl/pull/520 --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 10a0fef7b7db..acde4ebcc386 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 10a0fef7b7db749f5baf3384e9c900d3aa0e8140 +Subproject commit acde4ebcc38692213ee475c392f68ecb0b853818 From 48ba88eaf00208eed884fcae8209f64b0ddfa2ee Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 16:42:20 -0500 Subject: [PATCH 0033/1046] pwm_out_sim add mixer saturation status publication (#10866) --- src/drivers/pwm_out_sim/PWMSim.cpp | 15 +++++++++++++++ src/drivers/pwm_out_sim/PWMSim.hpp | 1 + 2 files changed, 16 insertions(+) diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index ef7054b3800e..2771c974a69b 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -33,6 +33,8 @@ #include "PWMSim.hpp" +#include + PWMSim::PWMSim() : CDev(PWM_OUTPUT0_DEVICE_PATH), _perf_control_latency(perf_alloc(PC_ELAPSED, "pwm_out_sim control latency")) @@ -285,6 +287,19 @@ PWMSim::run() } } + /* publish mixer status */ + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = _mixers->get_saturation_status(); + + if (saturation_status.flags.valid) { + multirotor_motor_limits_s motor_limits; + motor_limits.timestamp = hrt_absolute_time(); + motor_limits.saturation_status = saturation_status.value; + + int instance; + orb_publish_auto(ORB_ID(multirotor_motor_limits), &_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT); + } + /* and publish for anyone that cares to see */ _actuator_outputs.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs); diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 46772adf67e9..5804a466b016 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -112,6 +112,7 @@ class PWMSim : public cdev::CDev, public ModuleBase actuator_outputs_s _actuator_outputs = {}; orb_advert_t _outputs_pub{nullptr}; + orb_advert_t _mixer_status{nullptr}; unsigned _num_outputs{0}; From 26bac9594f1451f48a654da2e67c62f1c1950148 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 12:13:57 -0500 Subject: [PATCH 0034/1046] add simple SITL shutdown test --- platforms/posix/cmake/sitl_tests.cmake | 16 +++- posix-configs/SITL/init/test/test_shutdown | 95 ++++++++++++++++++++++ 2 files changed, 110 insertions(+), 1 deletion(-) create mode 100644 posix-configs/SITL/init/test/test_shutdown diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 96e9e5c77a7a..f8b29ceec0fb 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -76,9 +76,23 @@ add_test(NAME mavlink set_tests_properties(mavlink PROPERTIES FAIL_REGULAR_EXPRESSION "mavlink FAILED") set_tests_properties(mavlink PROPERTIES PASS_REGULAR_EXPRESSION "mavlink PASSED") - sanitizer_fail_test_on_error(mavlink) +# Shutdown test +add_test(NAME shutdown + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh + $ + none + none + test_shutdown + ${PX4_SOURCE_DIR} + ${PX4_BINARY_DIR} + WORKING_DIRECTORY ${SITL_WORKING_DIR}) + +#set_tests_properties(shutdown PROPERTIES FAIL_REGULAR_EXPRESSION "shutdown FAILED") +set_tests_properties(shutdown PROPERTIES PASS_REGULAR_EXPRESSION "Shutting down") +sanitizer_fail_test_on_error(shutdown) + # run arbitrary commands set(test_cmds hello diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown new file mode 100644 index 000000000000..86274883caca --- /dev/null +++ b/posix-configs/SITL/init/test/test_shutdown @@ -0,0 +1,95 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +uorb start + +param load + +param set BAT_N_CELLS 3 +param set MAV_TYPE 22 +param set VT_TYPE 2 +param set SYS_RESTART_TYPE 0 + +dataman start + +simulator start -t +tone_alarm start +gyrosim start +accelsim start +barosim start +gpssim start +measairspeedsim start +pwm_out_sim start + +sensors start +commander start +land_detector start vtol +navigator start +ekf2 start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +wind_estimator start + +#mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix + +ver all + +logger start -e -t + +mavlink boot_complete + +sleep 2 + +echo "Boot complete" + +# status of all running modules +echo "Checking status of all modules" +logger status +pwm_out_sim status +sensors status +commander status +land_detector status +navigator status +ekf2 status +mc_pos_control status +mc_att_control status +fw_pos_control_l1 status +fw_att_control status +wind_estimator status +dataman status +uorb status + +# stop all +echo "Stopping all modules" +logger stop +pwm_out_sim stop +mc_att_control stop +fw_att_control stop +mc_pos_control stop +fw_pos_control_l1 stop +navigator stop +commander stop +land_detector stop +ekf2 stop +wind_estimator stop +sensors stop + +sleep 2 + +simulator stop +tone_alarm stop +gyrosim stop +#accelsim stop +#barosim stop +gpssim stop +measairspeedsim stop + +dataman stop +#uorb stop + +shutdown From bba475aca19ecbcf83db1ea9c4fd3f5c919012c8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 16:18:35 -0500 Subject: [PATCH 0035/1046] logger limit trajectory_setpoint rate --- src/modules/logger/logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index bf92004165fd..b930dae9b5bb 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -641,7 +641,7 @@ void Logger::add_default_topics() add_topic("sensor_preflight", 200); add_topic("system_power", 500); add_topic("tecs_status", 200); - add_topic("trajectory_setpoint"); + add_topic("trajectory_setpoint", 200); add_topic("telemetry_status"); add_topic("vehicle_air_data", 200); add_topic("vehicle_attitude", 30); From 98394afcc7a81fb6d5c44b82f61eb2227a005640 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 08:12:56 -0700 Subject: [PATCH 0036/1046] board common:Define default CONTROL_STATUS LED mapping This commit is the groundwork to fix the power LED blinking on V5 Background: ---------- Early boards only had an AMBER LED that was used to indicate a High Load condition. This change defines the new logical inteface the LED_ should not be used in application code moving forward, only the minipulator macros should be used. Logical usage Legacy default ------------------------+------------- BOARD_OVERLOAD_LED | LED_RED Later boards defined BOARD_HAS_CONTROL_STATUS_LEDS and added the use of a BLUE and GREEN LED that were used as follows: Logical usage Legacy default ------------------------+------------- BOARD_ARMED_LED | LED_BLUE BOARD_ARMED_STATE_LED | LED_GREEN With this PR a board may now define _only_ a subset the leds and map them at the board level to the color LED it wants to use. Logical usage Legacy default ------------------------+------------- BOARD_OVERLOAD_LED | LED_RED when BOARD_HAS_CONTROL_STATUS_LEDS is defined BOARD_ARMED_LED | LED_BLUE BOARD_ARMED_STATE_LED | LED_GREEN If any of the BOARD_{OVERLOAD|ARMED|ARMED_STATE}_LED are not defined. The code output generates a null action for that LED. --- src/drivers/boards/common/board_common.h | 33 ++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h index fcf54bc535dc..12bcf0b294dd 100644 --- a/src/drivers/boards/common/board_common.h +++ b/src/drivers/boards/common/board_common.h @@ -291,6 +291,39 @@ # define BOARD_HAS_VERSIONING 1 #endif +/* Default LED logical to color mapping */ + +#if defined(BOARD_OVERLOAD_LED) +# define BOARD_OVERLOAD_LED_TOGGLE() led_toggle(BOARD_OVERLOAD_LED) +# define BOARD_OVERLOAD_LED_OFF() led_off(BOARD_OVERLOAD_LED) +#else +# define BOARD_OVERLOAD_LED_TOGGLE() +# define BOARD_OVERLOAD_LED_OFF() +#endif + +#if defined(BOARD_HAS_CONTROL_STATUS_LEDS) + +# if defined(BOARD_ARMED_LED) +# define BOARD_ARMED_LED_TOGGLE() led_toggle(BOARD_ARMED_LED) +# define BOARD_ARMED_LED_OFF() led_off(BOARD_ARMED_LED) +# define BOARD_ARMED_LED_ON() led_on(BOARD_ARMED_LED) +# else +# define BOARD_ARMED_LED_TOGGLE() +# define BOARD_ARMED_LED_OFF() +# define BOARD_ARMED_LED_ON() +# endif + +# if defined(BOARD_ARMED_STATE_LED) +# define BOARD_ARMED_STATE_LED_TOGGLE() led_toggle(BOARD_ARMED_STATE_LED) +# define BOARD_ARMED_STATE_LED_OFF() led_off(BOARD_ARMED_STATE_LED) +# define BOARD_ARMED_STATE_LED_ON() led_on(BOARD_ARMED_STATE_LED) +# else +# define BOARD_ARMED_STATE_LED_TOGGLE() +# define BOARD_ARMED_STATE_LED_OFF() +# define BOARD_ARMED_STATE_LED_ON() +# endif +#endif // + /************************************************************************************ * Public Data ************************************************************************************/ From dc29820bd97fcc601bb459920a2eca924051c6f4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 08:52:11 -0700 Subject: [PATCH 0037/1046] Document the LED colors used --- src/drivers/boards/aerofc-v1/led.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/aerofc-v1/led.c b/src/drivers/boards/aerofc-v1/led.c index f0004614a578..543814454be1 100644 --- a/src/drivers/boards/aerofc-v1/led.c +++ b/src/drivers/boards/aerofc-v1/led.c @@ -75,16 +75,16 @@ static uint32_t _led_param_get(int led) { switch (led) { case 0: - return GPIO_LED0; + return GPIO_LED0; // LED_BLUE case 1: - return GPIO_LED1; + return GPIO_LED1; // LED_RED case 2: - return GPIO_LED2; + return GPIO_LED2; // LED SAFETY case 3: - return GPIO_LED3; + return GPIO_LED3; // LED GREEN } From 0a1dd24af6944e200385357cf13551f7afd2ed6e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 08:53:19 -0700 Subject: [PATCH 0038/1046] omnibus-f4sd:led clean up code and prevent out of bounds indexing --- src/drivers/boards/omnibus-f4sd/led.c | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/src/drivers/boards/omnibus-f4sd/led.c b/src/drivers/boards/omnibus-f4sd/led.c index 06b3f7a75276..b1e75c444e55 100644 --- a/src/drivers/boards/omnibus-f4sd/led.c +++ b/src/drivers/boards/omnibus-f4sd/led.c @@ -75,7 +75,9 @@ __EXPORT void led_init(void) static void phy_set_led(int led, bool state) { /* Pull Down to switch on */ - stm32_gpiowrite(g_ledmap[led], !state); + if (led == 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } } __EXPORT void led_on(int led) @@ -88,6 +90,13 @@ __EXPORT void led_off(int led) phy_set_led(led, false); } +__EXPORT void led_toggle(int led) +{ + if (led == 0) { + phy_set_led(led, !stm32_gpioread(g_ledmap[led])); + } +} + __EXPORT void board_autoled_initialize(void) { /* Configure LED1 GPIO for output */ @@ -95,7 +104,6 @@ __EXPORT void board_autoled_initialize(void) } __EXPORT void board_autoled_on(int led) - { if (led == 1) { /* Pull down to switch on */ @@ -104,22 +112,9 @@ __EXPORT void board_autoled_on(int led) } __EXPORT void board_autoled_off(int led) - { if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } } - -__EXPORT void led_toggle(int led) -{ - if (led == 1) { - if (stm32_gpioread(GPIO_LED1)) { - stm32_gpiowrite(GPIO_LED1, false); - - } else { - stm32_gpiowrite(GPIO_LED1, true); - } - } -} From 96ba6c1174351f3f1d41de062c61088d85bf6296 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 08:55:48 -0700 Subject: [PATCH 0039/1046] sitl led:Support logical LED operations in commander add support for all the LEDS to support BOARD_HAS_CONTROL_STATUS_LEDS --- src/drivers/boards/sitl/sitl_led.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/sitl/sitl_led.c b/src/drivers/boards/sitl/sitl_led.c index 4c43ffc5788f..0f126f1c8890 100644 --- a/src/drivers/boards/sitl/sitl_led.c +++ b/src/drivers/boards/sitl/sitl_led.c @@ -40,6 +40,7 @@ #include #include #include +#include __BEGIN_DECLS extern void led_init(void); @@ -48,7 +49,7 @@ extern void led_off(int led); extern void led_toggle(int led); __END_DECLS -static bool _led_state[2] = { false, false }; +static bool _led_state[] = { false, false, false, false }; __EXPORT void led_init() { @@ -57,7 +58,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1 || led == 0) { + if ((unsigned int)led < arraySize(_led_state)) { PX4_DEBUG("LED%d_ON", led); _led_state[led] = true; } @@ -65,7 +66,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1 || led == 0) { + if ((unsigned int)led < arraySize(_led_state)) { PX4_DEBUG("LED%d_OFF", led); _led_state[led] = false; } @@ -73,7 +74,7 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1 || led == 0) { + if ((unsigned int)led < arraySize(_led_state)) { _led_state[led] = !_led_state[led]; PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); From aa30560d684fe32596419590e914b263fe518f0e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:44 -0700 Subject: [PATCH 0040/1046] aerofc-v1:Added Logical BOARD specific LED mapping Define the logical LED that are uses on aerofc-v1 --- src/drivers/boards/aerofc-v1/board_config.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/boards/aerofc-v1/board_config.h b/src/drivers/boards/aerofc-v1/board_config.h index 2ed5bb71a54c..9b18bd181ac1 100644 --- a/src/drivers/boards/aerofc-v1/board_config.h +++ b/src/drivers/boards/aerofc-v1/board_config.h @@ -63,6 +63,9 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN #define GPIO_VDD_5V_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN13) From e960978296ff0d13edfe681cddca2b847a2397f7 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:44 -0700 Subject: [PATCH 0041/1046] auav-x21:Added Logical BOARD specific LED mapping Define the logical LED that are uses on auav-x21 --- src/drivers/boards/auav-x21/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/auav-x21/board_config.h b/src/drivers/boards/auav-x21/board_config.h index 1cd1878f5da0..5ebeedba9650 100644 --- a/src/drivers/boards/auav-x21/board_config.h +++ b/src/drivers/boards/auav-x21/board_config.h @@ -71,6 +71,7 @@ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define BOARD_OVERLOAD_LED LED_RED /* External interrupts but on board*/ #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) From 3c6539257f1489f82a73f60a840b966899d492f8 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:44 -0700 Subject: [PATCH 0042/1046] crazyflie:Added Logical BOARD specific LED mapping Define the logical LED that are uses on crazyflie --- src/drivers/boards/crazyflie/board_config.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index 1cb3b1bf27b1..5fce4046c206 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -70,7 +70,10 @@ /* PX4: armed state indicator ; Stock FW: Blinking while charging */ #define GPIO_LED_BLUE_L (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN2) -#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN #define LED_TX 4 #define LED_RX 5 From a12f26aee0335aa9d977550201abd125a3ede87a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:45 -0700 Subject: [PATCH 0043/1046] mindpx-v2:Added Logical BOARD specific LED mapping Define the logical LED that are uses on mindpx-v2 --- src/drivers/boards/mindpx-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/mindpx-v2/board_config.h b/src/drivers/boards/mindpx-v2/board_config.h index b99f1626aa10..62bb6e8a54fe 100644 --- a/src/drivers/boards/mindpx-v2/board_config.h +++ b/src/drivers/boards/mindpx-v2/board_config.h @@ -56,6 +56,7 @@ /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define BOARD_OVERLOAD_LED LED_RED /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN4) From 1da0021be9c0b5e03c141ee286b7178119d984fb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:45 -0700 Subject: [PATCH 0044/1046] nxphlite-v3:Added Logical BOARD specific LED mapping Define the logical LED that are uses on nxphlite-v3 --- src/drivers/boards/nxphlite-v3/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/nxphlite-v3/board_config.h b/src/drivers/boards/nxphlite-v3/board_config.h index 45479bf075f1..e3ab307594b7 100644 --- a/src/drivers/boards/nxphlite-v3/board_config.h +++ b/src/drivers/boards/nxphlite-v3/board_config.h @@ -73,6 +73,8 @@ __BEGIN_DECLS #define GPIO_LED_2 (GPIO_HIGHDRIVE | GPIO_OUTPUT_ZER0 | PIN_PORTD | PIN14) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 // Use D9 and D10 +#define BOARD_OVERLOAD_LED LED_AMBER +#define BOARD_ARMED_STATE_LED LED_GREEN #define GPIO_NFC_IO (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTA | PIN26) #define GPIO_SENSOR_P_EN (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE | PIN_PORTB | PIN8) From 42a8a6ffac787ca19fea301cda6d702cd0a9e9cd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:45 -0700 Subject: [PATCH 0045/1046] omnibus-f4sd:Added Logical BOARD specific LED mapping Define the logical LED that are uses on omnibus-f4sd --- src/drivers/boards/omnibus-f4sd/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/omnibus-f4sd/board_config.h b/src/drivers/boards/omnibus-f4sd/board_config.h index 589a6f2dc11b..26f4f08be74b 100644 --- a/src/drivers/boards/omnibus-f4sd/board_config.h +++ b/src/drivers/boards/omnibus-f4sd/board_config.h @@ -59,6 +59,8 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) #define GPIO_LED_BLUE GPIO_LED1 +#define BOARD_OVERLOAD_LED LED_BLUE + /* * ADC channels * From 03aae61395a84965fe70f243813ab54665593ada Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:46 -0700 Subject: [PATCH 0046/1046] px4-same70xplained-v1:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4-same70xplained-v1 --- src/drivers/boards/px4-same70xplained-v1/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4-same70xplained-v1/board_config.h b/src/drivers/boards/px4-same70xplained-v1/board_config.h index cd041a2890bd..45dfcc80221d 100644 --- a/src/drivers/boards/px4-same70xplained-v1/board_config.h +++ b/src/drivers/boards/px4-same70xplained-v1/board_config.h @@ -99,6 +99,8 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT | GPIO_CFG_DEFAULT | GPIO_OUTPUT_SET | \ GPIO_PORT_PIOC | GPIO_PIN8) +#define GPIO_LED_RED GPIO_LED1 +#define BOARD_OVERLOAD_LED LED_RED /* Buttons * From e40a0839e96157a7cc345b69005a1ec7e5008c69 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:46 -0700 Subject: [PATCH 0047/1046] px4-stm32f4discovery:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4-stm32f4discovery --- src/drivers/boards/px4-stm32f4discovery/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index ba2fea7c62bb..adc74445034d 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -66,6 +66,8 @@ #define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define BOARD_OVERLOAD_LED LED_RED + /* SPI chip selects */ #define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) From 90106c1fd2fe97541fce9b7dee04272b111ff66e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:46 -0700 Subject: [PATCH 0048/1046] px4fmu-v2:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4fmu-v2 --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 38f38bfb92dd..e3da7692c227 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -85,6 +85,7 @@ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define BOARD_OVERLOAD_LED LED_AMBER /* * Define the ability to shut off off the sensor signals From 735830a483b4f5d240c02abb6526f5ca4c1c423d Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:46 -0700 Subject: [PATCH 0049/1046] px4fmu-v4:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4fmu-v4 --- src/drivers/boards/px4fmu-v4/board_config.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 599874828cc7..95b9c9d9dce8 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -62,7 +62,10 @@ #define GPIO_LED_GREEN GPIO_LED2 #define GPIO_LED_BLUE GPIO_LED3 -#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN /** * Define the Chip Selects for SPI1 From 26026b5d395157efe5be9dbb731fd1767c1b6cc7 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:47 -0700 Subject: [PATCH 0050/1046] px4fmu-v4pro:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4fmu-v4pro --- src/drivers/boards/px4fmu-v4pro/board_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4fmu-v4pro/board_config.h b/src/drivers/boards/px4fmu-v4pro/board_config.h index 9f824ee38289..7b8913bf6f4d 100644 --- a/src/drivers/boards/px4fmu-v4pro/board_config.h +++ b/src/drivers/boards/px4fmu-v4pro/board_config.h @@ -87,6 +87,11 @@ #define GPIO_LED_GREEN GPIO_LED2 #define GPIO_LED_BLUE GPIO_LED3 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + /* Define the Chip Selects */ /* SPI Bus 1 Internal Sensors */ From b37675fa762e96cdc6dbbb2a59072f8f86035ad3 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:47 -0700 Subject: [PATCH 0051/1046] px4fmu-v5:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4fmu-v5 --- src/drivers/boards/px4fmu-v5/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v5/board_config.h b/src/drivers/boards/px4fmu-v5/board_config.h index dc426a261200..f501cefbe624 100644 --- a/src/drivers/boards/px4fmu-v5/board_config.h +++ b/src/drivers/boards/px4fmu-v5/board_config.h @@ -99,7 +99,8 @@ #define GPIO_nLED_BLUE /* PC7 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 - +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE /* SENSORS are on SPI1, 5, 6 * MEMORY is on bus SPI2 From 05ea187b1df8f7bda2038da3d281031cea4dbc83 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:47 -0700 Subject: [PATCH 0052/1046] px4nucleoF767ZI-v1:Added Logical BOARD specific LED mapping Define the logical LED that are uses on px4nucleoF767ZI-v1 --- src/drivers/boards/px4nucleoF767ZI-v1/board_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4nucleoF767ZI-v1/board_config.h b/src/drivers/boards/px4nucleoF767ZI-v1/board_config.h index 6c455845b4d5..0732fb048bc1 100644 --- a/src/drivers/boards/px4nucleoF767ZI-v1/board_config.h +++ b/src/drivers/boards/px4nucleoF767ZI-v1/board_config.h @@ -79,6 +79,11 @@ __BEGIN_DECLS #define GPIO_LED_GREEN GPIO_LED2 #define GPIO_LED_BLUE GPIO_LED3 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + /* Define the Chip Selects */ #define GPIO_SPI_CS_MPU9250 /* PF2[CN11-52] */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN2) From fbc143dc55307e69d96222084921e7fbb34a2c81 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 09:05:48 -0700 Subject: [PATCH 0053/1046] sitl:Added Logical BOARD specific LED mapping Define the logical LED that are uses on sitl --- src/drivers/boards/sitl/board_config.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/boards/sitl/board_config.h b/src/drivers/boards/sitl/board_config.h index ca3fb9c0946f..ae651192c968 100644 --- a/src/drivers/boards/sitl/board_config.h +++ b/src/drivers/boards/sitl/board_config.h @@ -55,6 +55,10 @@ #define PX4_NUMBER_I2C_BUSES 1 #define BOARD_NUMBER_BRICKS 0 +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN #include #include "../common/board_common.h" From 1d12827408596a611fe0d1fc183ea822867770bc Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 26 Sep 2018 08:46:11 -0700 Subject: [PATCH 0054/1046] Commander:Replace color based LED manipulators with logical ones. --- src/modules/commander/Commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 53fc25f1ad21..d484acf0ed0a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2644,33 +2644,33 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (actuator_armed->armed) { if (status.failsafe) { - led_off(LED_BLUE); + BOARD_ARMED_LED_OFF(); if (leds_counter % 5 == 0) { - led_toggle(LED_GREEN); + BOARD_ARMED_STATE_LED_TOGGLE(); } } else { - led_off(LED_GREEN); + BOARD_ARMED_STATE_LED_OFF(); /* armed, solid */ - led_on(LED_BLUE); + BOARD_ARMED_LED_ON(); } } else if (actuator_armed->ready_to_arm) { - led_off(LED_BLUE); + BOARD_ARMED_LED_OFF(); /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) { - led_toggle(LED_GREEN); + BOARD_ARMED_STATE_LED_TOGGLE(); } } else { - led_off(LED_BLUE); + BOARD_ARMED_LED_OFF(); /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) { - led_toggle(LED_GREEN); + BOARD_ARMED_STATE_LED_TOGGLE(); } } @@ -2679,11 +2679,11 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* give system warnings on error LED */ if (overload) { if (leds_counter % 2 == 0) { - led_toggle(LED_AMBER); + BOARD_OVERLOAD_LED_TOGGLE(); } } else { - led_off(LED_AMBER); + BOARD_OVERLOAD_LED_OFF(); } leds_counter++; From db514fe4418173ebf1118b6559733c7fec1741f7 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 30 Oct 2018 16:16:56 +0100 Subject: [PATCH 0055/1046] Add a collision avoidance library which uses range data --- msg/CMakeLists.txt | 1 + msg/collision_constraints.msg | 12 ++ src/lib/CMakeLists.txt | 1 + src/lib/CollisionAvoidance/CMakeLists.txt | 34 ++++ .../CollisionAvoidance/CollisionAvoidance.cpp | 175 ++++++++++++++++++ .../CollisionAvoidance/CollisionAvoidance.hpp | 110 +++++++++++ src/lib/FlightTasks/FlightTasks.hpp | 6 + .../tasks/FlightTask/FlightTask.hpp | 7 + .../FlightTaskManualPosition.cpp | 6 + .../FlightTaskManualPosition.hpp | 7 + src/modules/logger/logger.cpp | 2 + src/modules/mc_pos_control/CMakeLists.txt | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 34 ++++ .../mc_pos_control/mc_pos_control_params.c | 19 ++ 14 files changed, 415 insertions(+) create mode 100644 msg/collision_constraints.msg create mode 100644 src/lib/CollisionAvoidance/CMakeLists.txt create mode 100644 src/lib/CollisionAvoidance/CollisionAvoidance.cpp create mode 100644 src/lib/CollisionAvoidance/CollisionAvoidance.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5780155febd8..c66a74858756 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -42,6 +42,7 @@ set(msg_files camera_capture.msg camera_trigger.msg collision_report.msg + collision_constraints.msg commander_state.msg cpuload.msg debug_array.msg diff --git a/msg/collision_constraints.msg b/msg/collision_constraints.msg new file mode 100644 index 000000000000..4c08ac801ddd --- /dev/null +++ b/msg/collision_constraints.msg @@ -0,0 +1,12 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement. +float32[2] constraints_normalized_x # constraints determined by range sensor measurements [x negative, x positive] +float32[2] constraints_normalized_y # constraintss determined by range sensor measurements [y negative, y positive] + +float32[2] original_setpoint # velocities demanded +float32[2] adapted_setpoint # velocities allowed + diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 45956ee7f8b5..e440cf07d971 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,3 +56,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(WeatherVane) +add_subdirectory(CollisionAvoidance) diff --git a/src/lib/CollisionAvoidance/CMakeLists.txt b/src/lib/CollisionAvoidance/CMakeLists.txt new file mode 100644 index 000000000000..8bf01178bbe0 --- /dev/null +++ b/src/lib/CollisionAvoidance/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(CollisionAvoidance CollisionAvoidance.cpp) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp new file mode 100644 index 000000000000..823719c0c390 --- /dev/null +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CollisionAvoidance.cpp + * CollisionAvoidance controller. + * + */ + +#include "CollisionAvoidance.hpp" + + +CollisionAvoidance::CollisionAvoidance() : + ModuleParams(nullptr) +{ + +} + +void CollisionAvoidance::reset_constraints() +{ + + _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction + _move_constraints_x_normalized(1) = 0.0f; //normalized constraint in positive x-direction + _move_constraints_y_normalized(0) = 0.0f; //normalized constraint in negative y-direction + _move_constraints_y_normalized(1) = 0.0f; //normalized constraint in positive y-direction + + _move_constraints_x(0) = 0.0f; //constraint in negative x-direction + _move_constraints_x(1) = 0.0f; //constraint in positive x-direction + _move_constraints_y(0) = 0.0f; //constraint in negative y-direction + _move_constraints_y(1) = 0.0f; //constraint in positive y-direction + +} + +void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) +{ + + collision_constraints_s constraints; /**< collision constraints message */ + + //fill in values + constraints.timestamp = hrt_absolute_time(); + constraints.constraints_normalized_x[0] = _move_constraints_x_normalized(0); + constraints.constraints_normalized_x[1] = _move_constraints_x_normalized(1); + constraints.constraints_normalized_y[0] = _move_constraints_y_normalized(0); + constraints.constraints_normalized_y[1] = _move_constraints_y_normalized(1); + + constraints.original_setpoint[0] = original_setpoint(0); + constraints.original_setpoint[1] = original_setpoint(1); + constraints.adapted_setpoint[0] = adapted_setpoint(0); + constraints.adapted_setpoint[1] = adapted_setpoint(1); + + // publish constraints + if (_constraints_pub != nullptr) { + orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints); + + } else { + _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); + } + +} + +void CollisionAvoidance::update_range_constraints() +{ + if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + float max_detection_distance = _obstacle_distance.max_distance / 100.0f; //convert to meters + + for (int i = 0; i < 72; i++) { + //determine if distance bin is valid and contains a valid distance measurement + if (_obstacle_distance.distances[i] < _obstacle_distance.max_distance && + _obstacle_distance.distances[i] > _obstacle_distance.min_distance && i * _obstacle_distance.increment < 360) { + float distance = _obstacle_distance.distances[i] / 100.0f; //convert to meters + float angle = i * _obstacle_distance.increment * (M_PI / 180.0); + //calculate normalized velocity reductions + float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * cos(angle); + float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * sin(angle); + + if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; } + + if (vel_lim_y > 0 && vel_lim_y > _move_constraints_y_normalized(1)) { _move_constraints_y_normalized(1) = vel_lim_y; } + + if (vel_lim_x < 0 && -vel_lim_x > _move_constraints_x_normalized(0)) { _move_constraints_x_normalized(0) = -vel_lim_x; } + + if (vel_lim_y < 0 && -vel_lim_y > _move_constraints_y_normalized(0)) { _move_constraints_y_normalized(0) = -vel_lim_y; } + } + } + + } else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) { + mavlink_log_critical(&_mavlink_log_pub, "No range data received"); + _last_message = hrt_absolute_time(); + } +} + +void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) +{ + + reset_constraints(); + + //calculate movement constraints based on range data + update_range_constraints(); + _move_constraints_x = _move_constraints_x_normalized; + _move_constraints_y = _move_constraints_y_normalized; + + // calculate the maximum velocity along x,y axis when moving in the demanded direction + float vel_mag = sqrt(original_setpoint(0) * original_setpoint(0) + original_setpoint(1) * original_setpoint(1)); + float v_max_x, v_max_y; + + if (vel_mag > 0.0f) { + v_max_x = abs(max_speed / vel_mag * original_setpoint(0)); + v_max_y = abs(max_speed / vel_mag * original_setpoint(1)); + + } else { + v_max_x = 0.0f; + v_max_y = 0.0f; + } + + //scale the velocity reductions with the maximum possible velocity along the respective axis + _move_constraints_x(0) *= v_max_x; + _move_constraints_x(1) *= v_max_x; + _move_constraints_y(0) *= v_max_y; + _move_constraints_y(1) *= v_max_y; + + //apply the velocity reductions to form velocity limits + _move_constraints_x(0) = v_max_x - _move_constraints_x(0); + _move_constraints_x(1) = v_max_x - _move_constraints_x(1); + _move_constraints_y(0) = v_max_y - _move_constraints_y(0); + _move_constraints_y(1) = v_max_y - _move_constraints_y(1); + + //constrain the velocity setpoint to respect the velocity limits + Vector2f new_setpoint = original_setpoint; + new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1)); + new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1)); + + //warn user if collision avoidance starts to interfere + bool currently_interfering = (new_setpoint(0) < 0.95f * original_setpoint(0) + || new_setpoint(0) > 1.05f * original_setpoint(0) || new_setpoint(1) < 0.95f * original_setpoint(1) + || new_setpoint(1) > 1.05f * original_setpoint(1)); + + if (currently_interfering && (currently_interfering != _interfering)) { + mavlink_log_critical(&_mavlink_log_pub, "Collision Avoidance starts interfering"); + } + + _interfering = currently_interfering; + + publish_constraints(original_setpoint, new_setpoint); + original_setpoint = new_setpoint; +} diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp new file mode 100644 index 000000000000..bf1f5539c219 --- /dev/null +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CollisionAvoidance.hpp + * @author Tanja Baumann + * + * CollisionAvoidance controller. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +using uORB::Publication; +#include +#include + +using namespace matrix; +using namespace time_literals; + +class CollisionAvoidance : public ModuleParams +{ +public: + CollisionAvoidance(); + + ~CollisionAvoidance() = default; + + void activate() {_is_active = true;} + + void deactivate() {_is_active = false;} + + bool is_active() {return _is_active;} + + bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } + + void update(const obstacle_distance_s &distance_measurements) {_obstacle_distance = distance_measurements;} + + void update_range_constraints(); + + void reset_constraints(); + + void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + + void modifySetpoint(Vector2f &original_setpoint, const float max_speed); + +private: + + obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ + + bool _is_active = true; + bool _interfering = false; /**< states if the collision avoidance interferes with the user input */ + + orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ + orb_advert_t _mavlink_log_pub = nullptr; /**< Mavlink log uORB handle */ + + static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; + static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s; + + hrt_abstime _last_message; + + Vector2f _move_constraints_x_normalized; + Vector2f _move_constraints_y_normalized; + Vector2f _move_constraints_x; + Vector2f _move_constraints_y; + + DEFINE_PARAMETERS( + (ParamInt) MPC_COL_AVOID, /**< use range sensor measurements to avoid collision */ + (ParamFloat) MPC_COL_AVOID_D /**< collision avoidance keep minimum distance */ + ) + +}; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 3a07d25ae349..5cca46e0626f 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,6 +45,7 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include +#include #include @@ -131,6 +132,11 @@ class FlightTasks */ void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} + /** + * Sets an external collision avoidance. The active flight task can use the the collision avoidance to modify the setpoint. + */ + void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {_current_task.task->setCollisionAvoidance(ext_collision_avoidance);} + /** * This method will re-activate current task. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 9efe587ee47d..30ef82a3fde8 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include "SubscriptionArray.hpp" class FlightTask : public ModuleParams @@ -154,6 +155,12 @@ class FlightTask : public ModuleParams void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } + + /** + * Sets an external collision avoidance which can be used by any flight task to implement a different setpoint + * This method does nothing, each flighttask which wants to use the collision avoidance needs to override this method. + */ + virtual void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {}; protected: diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 12bc4586dea8..f61e8728d985 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -113,6 +113,12 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); + + //collision avoidance + if (_ext_collision_avoidance != nullptr && _ext_collision_avoidance->is_active()) { + _ext_collision_avoidance->modifySetpoint(vel_sp_xy, _constraints.speed_xy); + } + _velocity_setpoint(0) = vel_sp_xy(0); _velocity_setpoint(1) = vel_sp_xy(1); } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 04a2064fdfa1..337dbe1d3146 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -56,6 +56,12 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude */ void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + /** + * Sets an external collision avoidance which can be used to modify setpoints + */ + void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) override {_ext_collision_avoidance = ext_collision_avoidance;} + + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -74,4 +80,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + CollisionAvoidance *_ext_collision_avoidance = nullptr; /**< external collision avoidance library*/ }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b930dae9b5bb..fbd65614f463 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -622,6 +622,7 @@ void Logger::add_default_topics() add_topic("battery_status", 500); add_topic("camera_capture"); add_topic("camera_trigger"); + add_topic("collision_constraints"); add_topic("cpuload"); add_topic("distance_sensor", 100); add_topic("ekf2_innovations", 200); @@ -633,6 +634,7 @@ void Logger::add_default_topics() add_topic("manual_control_setpoint", 200); add_topic("mission"); add_topic("mission_result"); + add_topic("obstacle_distance"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); add_topic("radio_status"); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index f60e2abe39a7..247a266a6c4a 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -45,4 +45,5 @@ px4_add_module( git_ecl ecl_geo WeatherVane + CollisionAvoidance ) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0b50c5d7b4c2..9a513393e74e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -64,6 +65,7 @@ #include #include +#include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" @@ -117,6 +119,7 @@ class MulticopterPositionControl : public ModuleBase int _att_sub{-1}; /**< vehicle attitude */ int _home_pos_sub{-1}; /**< home position */ int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */ + int _range_sensor_sub{-1}; /**< obstacle distances */ int _task_failure_count{0}; /**< counter for task failures */ @@ -133,6 +136,7 @@ class MulticopterPositionControl : public ModuleBase home_position_s _home_pos{}; /**< home position */ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ + obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -181,6 +185,7 @@ class MulticopterPositionControl : public ModuleBase systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; + CollisionAvoidance *_ca_controller{nullptr}; /** * Update our local parameter cache. @@ -355,6 +360,9 @@ MulticopterPositionControl::~MulticopterPositionControl() if (_wv_controller != nullptr) { delete _wv_controller; } + if (_ca_controller != nullptr) { + delete _ca_controller; + } } void @@ -425,6 +433,10 @@ MulticopterPositionControl::poll_subscriptions() if (_wv_controller == nullptr && _vehicle_status.is_vtol) { _wv_controller = new WeatherVane(); } + // if the vehicle is a rotary wing, enable collision avoidance capabilities + if (_ca_controller == nullptr && _vehicle_status.is_rotary_wing) { + _ca_controller = new CollisionAvoidance(); + } } orb_check(_vehicle_land_detected_sub, &updated); @@ -465,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance); } + + orb_check(_range_sensor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(obstacle_distance), _range_sensor_sub, &_obstacle_distance); + } } void @@ -582,6 +600,7 @@ MulticopterPositionControl::run() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint)); + _range_sensor_sub = orb_subscribe(ORB_ID(obstacle_distance)); parameters_update(true); @@ -644,6 +663,20 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } + // activate the collision avoidance if required. If activated a flighttask can use it modify the setpoint + if (_ca_controller != nullptr) { + + // in manual mode we just want to use weathervane if position is controlled as well + if (_ca_controller->collision_avoidance_enabled()) { + _ca_controller->activate(); + + } else { + _ca_controller->deactivate(); + } + + _ca_controller->update(_obstacle_distance); + } + if (_control_mode.flag_armed) { // as soon vehicle is armed check for flighttask start_flight_task(); @@ -665,6 +698,7 @@ MulticopterPositionControl::run() vehicle_local_position_setpoint_s setpoint; _flight_tasks.setYawHandler(_wv_controller); + _flight_tasks.setCollisionAvoidance(_ca_controller); // update task if (!_flight_tasks.update()) { diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 4ac55f0decf5..b09af23e3b57 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -737,3 +737,22 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * Flag to enable the use of a range sensor for collision avoidance. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); +/** + * Minimum Obstacle Distance at which the vehicle should not get closer + * + * Only relevant if in Position control and the range sensor is active + * + * @min 0 + * @max 15 + * @unit meters + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_AVOID_D, 4.0f); From ce7dc854501eeebf294f046427e2111aaf2c41d1 Mon Sep 17 00:00:00 2001 From: baumanta Date: Fri, 2 Nov 2018 15:09:41 +0100 Subject: [PATCH 0056/1046] Change wrong comment --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9a513393e74e..6e0eb06bac6a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -663,7 +663,7 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // activate the collision avoidance if required. If activated a flighttask can use it modify the setpoint + // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (_ca_controller != nullptr) { // in manual mode we just want to use weathervane if position is controlled as well From 27dc60a41953015650044d04c989e8b2cd9725a6 Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 5 Nov 2018 08:42:30 +0100 Subject: [PATCH 0057/1046] Improve comments --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_params.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 823719c0c390..712286579279 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -165,7 +165,7 @@ void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float || new_setpoint(1) > 1.05f * original_setpoint(1)); if (currently_interfering && (currently_interfering != _interfering)) { - mavlink_log_critical(&_mavlink_log_pub, "Collision Avoidance starts interfering"); + mavlink_log_critical(&_mavlink_log_pub, "Collision Warning"); } _interfering = currently_interfering; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index b09af23e3b57..6d9e8ad1ac15 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -739,16 +739,16 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); /** - * Flag to enable the use of a range sensor for collision avoidance. + * Flag to enable the use of a MAVLink range sensor for collision avoidance. * * @boolean * @group Multicopter Position Control */ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); /** - * Minimum Obstacle Distance at which the vehicle should not get closer + * Minimum distance the vehicle should keep to all obstacles * - * Only relevant if in Position control and the range sensor is active + * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). * * @min 0 * @max 15 From 7e276bc16310d4cae92e511a0207d6209052f7d7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Nov 2018 21:44:15 +0100 Subject: [PATCH 0058/1046] CollisionAvoidance: move activation logic to library --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 12 ++++++++++++ src/lib/CollisionAvoidance/CollisionAvoidance.hpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 712286579279..f8a18304a6ea 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -88,6 +88,18 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } +void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements) { + // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter + if (collision_avoidance_enabled()) { + activate(); + + } else { + deactivate(); + } + + _obstacle_distance = distance_measurements; +} + void CollisionAvoidance::update_range_constraints() { if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index bf1f5539c219..53b3a8b65262 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -72,7 +72,7 @@ class CollisionAvoidance : public ModuleParams bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } - void update(const obstacle_distance_s &distance_measurements) {_obstacle_distance = distance_measurements;} + void update(const obstacle_distance_s &distance_measurements); void update_range_constraints(); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6e0eb06bac6a..abcd1a0466c6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -663,17 +663,7 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (_ca_controller != nullptr) { - - // in manual mode we just want to use weathervane if position is controlled as well - if (_ca_controller->collision_avoidance_enabled()) { - _ca_controller->activate(); - - } else { - _ca_controller->deactivate(); - } - _ca_controller->update(_obstacle_distance); } From 81ec6c5b1e95c5db6c8e11733cf07ec68782045b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Nov 2018 22:33:52 +0100 Subject: [PATCH 0059/1046] ColisionAvoidance: move instantiation to FlightTask --- .../CollisionAvoidance/CollisionAvoidance.cpp | 30 ++++++++++++------- .../CollisionAvoidance/CollisionAvoidance.hpp | 13 ++++++-- src/lib/FlightTasks/FlightTasks.hpp | 6 ---- .../tasks/FlightTask/FlightTask.hpp | 7 ----- .../FlightTaskManualPosition.cpp | 19 ++++++++++-- .../FlightTaskManualPosition.hpp | 9 ++---- .../mc_pos_control/mc_pos_control_main.cpp | 24 --------------- .../mc_pos_control/mc_pos_control_params.c | 1 + 8 files changed, 50 insertions(+), 59 deletions(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index f8a18304a6ea..1afc2f5c2687 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -46,6 +46,15 @@ CollisionAvoidance::CollisionAvoidance() : } +bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { + return false; + } + + return true; +} + void CollisionAvoidance::reset_constraints() { @@ -88,7 +97,8 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } -void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements) { +void CollisionAvoidance::update() +{ // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter if (collision_avoidance_enabled()) { activate(); @@ -96,21 +106,21 @@ void CollisionAvoidance::update(const obstacle_distance_s &distance_measurements } else { deactivate(); } - - _obstacle_distance = distance_measurements; } void CollisionAvoidance::update_range_constraints() { - if (hrt_elapsed_time((hrt_abstime *)&_obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { - float max_detection_distance = _obstacle_distance.max_distance / 100.0f; //convert to meters + obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get(); + + if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + float max_detection_distance = obstacle_distance.max_distance / 100.0f; //convert to meters for (int i = 0; i < 72; i++) { //determine if distance bin is valid and contains a valid distance measurement - if (_obstacle_distance.distances[i] < _obstacle_distance.max_distance && - _obstacle_distance.distances[i] > _obstacle_distance.min_distance && i * _obstacle_distance.increment < 360) { - float distance = _obstacle_distance.distances[i] / 100.0f; //convert to meters - float angle = i * _obstacle_distance.increment * (M_PI / 180.0); + if (obstacle_distance.distances[i] < obstacle_distance.max_distance && + obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) { + float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters + float angle = i * obstacle_distance.increment * (M_PI / 180.0); //calculate normalized velocity reductions float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * cos(angle); float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * sin(angle); @@ -133,7 +143,7 @@ void CollisionAvoidance::update_range_constraints() void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { - + update(); reset_constraints(); //calculate movement constraints based on range data diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index 53b3a8b65262..39a05c9bd0c8 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -53,6 +53,7 @@ using uORB::Publication; #include #include +#include using namespace matrix; using namespace time_literals; @@ -64,6 +65,12 @@ class CollisionAvoidance : public ModuleParams ~CollisionAvoidance() = default; + /** + * Initialize the uORB subscriptions using an array + * @return true on success, false on error + */ + bool initializeSubscriptions(SubscriptionArray &subscription_array); + void activate() {_is_active = true;} void deactivate() {_is_active = false;} @@ -72,7 +79,7 @@ class CollisionAvoidance : public ModuleParams bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } - void update(const obstacle_distance_s &distance_measurements); + void update(); void update_range_constraints(); @@ -84,14 +91,14 @@ class CollisionAvoidance : public ModuleParams private: - obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ - bool _is_active = true; bool _interfering = false; /**< states if the collision avoidance interferes with the user input */ orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ orb_advert_t _mavlink_log_pub = nullptr; /**< Mavlink log uORB handle */ + uORB::Subscription *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ + static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 5cca46e0626f..3a07d25ae349 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,7 +45,6 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include -#include #include @@ -132,11 +131,6 @@ class FlightTasks */ void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} - /** - * Sets an external collision avoidance. The active flight task can use the the collision avoidance to modify the setpoint. - */ - void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {_current_task.task->setCollisionAvoidance(ext_collision_avoidance);} - /** * This method will re-activate current task. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 30ef82a3fde8..9efe587ee47d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -52,7 +52,6 @@ #include #include #include -#include #include "SubscriptionArray.hpp" class FlightTask : public ModuleParams @@ -155,12 +154,6 @@ class FlightTask : public ModuleParams void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } - - /** - * Sets an external collision avoidance which can be used by any flight task to implement a different setpoint - * This method does nothing, each flighttask which wants to use the collision avoidance needs to override this method. - */ - virtual void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) {}; protected: diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index f61e8728d985..74457e8e929d 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,6 +41,19 @@ using namespace matrix; +bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { + return false; + } + + if (!_collision_avoidance.initializeSubscriptions(subscription_array)) { + return false; + } + + return true; +} + bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); @@ -114,9 +127,9 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); - //collision avoidance - if (_ext_collision_avoidance != nullptr && _ext_collision_avoidance->is_active()) { - _ext_collision_avoidance->modifySetpoint(vel_sp_xy, _constraints.speed_xy); + // collision avoidance + if (_collision_avoidance.is_active()) { + _collision_avoidance.modifySetpoint(vel_sp_xy, _constraints.speed_xy); } _velocity_setpoint(0) = vel_sp_xy(0); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 337dbe1d3146..59da9ee05745 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -41,6 +41,7 @@ #pragma once #include "FlightTaskManualAltitude.hpp" +#include class FlightTaskManualPosition : public FlightTaskManualAltitude { @@ -48,6 +49,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude FlightTaskManualPosition() = default; virtual ~FlightTaskManualPosition() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate() override; bool updateInitialize() override; @@ -56,11 +58,6 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude */ void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } - /** - * Sets an external collision avoidance which can be used to modify setpoints - */ - void setCollisionAvoidance(CollisionAvoidance *ext_collision_avoidance) override {_ext_collision_avoidance = ext_collision_avoidance;} - protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ @@ -80,5 +77,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionAvoidance *_ext_collision_avoidance = nullptr; /**< external collision avoidance library*/ + CollisionAvoidance _collision_avoidance; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index abcd1a0466c6..0b50c5d7b4c2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,7 +47,6 @@ #include #include -#include #include #include #include @@ -65,7 +64,6 @@ #include #include -#include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" @@ -119,7 +117,6 @@ class MulticopterPositionControl : public ModuleBase int _att_sub{-1}; /**< vehicle attitude */ int _home_pos_sub{-1}; /**< home position */ int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */ - int _range_sensor_sub{-1}; /**< obstacle distances */ int _task_failure_count{0}; /**< counter for task failures */ @@ -136,7 +133,6 @@ class MulticopterPositionControl : public ModuleBase home_position_s _home_pos{}; /**< home position */ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ - obstacle_distance_s _obstacle_distance{}; /**< obstacle distances received form a range sensor */ DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -185,7 +181,6 @@ class MulticopterPositionControl : public ModuleBase systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; - CollisionAvoidance *_ca_controller{nullptr}; /** * Update our local parameter cache. @@ -360,9 +355,6 @@ MulticopterPositionControl::~MulticopterPositionControl() if (_wv_controller != nullptr) { delete _wv_controller; } - if (_ca_controller != nullptr) { - delete _ca_controller; - } } void @@ -433,10 +425,6 @@ MulticopterPositionControl::poll_subscriptions() if (_wv_controller == nullptr && _vehicle_status.is_vtol) { _wv_controller = new WeatherVane(); } - // if the vehicle is a rotary wing, enable collision avoidance capabilities - if (_ca_controller == nullptr && _vehicle_status.is_rotary_wing) { - _ca_controller = new CollisionAvoidance(); - } } orb_check(_vehicle_land_detected_sub, &updated); @@ -477,12 +465,6 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance); } - - orb_check(_range_sensor_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(obstacle_distance), _range_sensor_sub, &_obstacle_distance); - } } void @@ -600,7 +582,6 @@ MulticopterPositionControl::run() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint)); - _range_sensor_sub = orb_subscribe(ORB_ID(obstacle_distance)); parameters_update(true); @@ -663,10 +644,6 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - if (_ca_controller != nullptr) { - _ca_controller->update(_obstacle_distance); - } - if (_control_mode.flag_armed) { // as soon vehicle is armed check for flighttask start_flight_task(); @@ -688,7 +665,6 @@ MulticopterPositionControl::run() vehicle_local_position_setpoint_s setpoint; _flight_tasks.setYawHandler(_wv_controller); - _flight_tasks.setCollisionAvoidance(_ca_controller); // update task if (!_flight_tasks.update()) { diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 6d9e8ad1ac15..a2e1d045429c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -745,6 +745,7 @@ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); * @group Multicopter Position Control */ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); + /** * Minimum distance the vehicle should keep to all obstacles * From f611eddadde74138290ad684a56bf7896e4d47cf Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 6 Nov 2018 10:24:41 +0100 Subject: [PATCH 0060/1046] unadvertise publishers in the destructor --- src/lib/CollisionAvoidance/CollisionAvoidance.cpp | 10 ++++++++++ src/lib/CollisionAvoidance/CollisionAvoidance.hpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp index 1afc2f5c2687..9b712cc553b6 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.cpp @@ -46,6 +46,16 @@ CollisionAvoidance::CollisionAvoidance() : } +CollisionAvoidance::~CollisionAvoidance(){ + //unadvertise publishers + if (_constraints_pub != nullptr) { + orb_unadvertise(_constraints_pub); + } + if (_mavlink_log_pub != nullptr) { + orb_unadvertise(_mavlink_log_pub); + } +} + bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp index 39a05c9bd0c8..66a72d75d4d3 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionAvoidance/CollisionAvoidance.hpp @@ -63,7 +63,7 @@ class CollisionAvoidance : public ModuleParams public: CollisionAvoidance(); - ~CollisionAvoidance() = default; + ~CollisionAvoidance(); /** * Initialize the uORB subscriptions using an array From dd45fa65412f9ba13e2fbe05186850ad2c53f0a0 Mon Sep 17 00:00:00 2001 From: baumanta Date: Wed, 7 Nov 2018 10:10:56 +0100 Subject: [PATCH 0061/1046] change feature name from CollisionAvoidance to CollisionPrevention --- src/lib/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../CollisionPrevention.cpp} | 34 ++++++++++--------- .../CollisionPrevention.hpp} | 18 +++++----- .../FlightTaskManualPosition.cpp | 8 ++--- .../FlightTaskManualPosition.hpp | 4 +-- src/modules/mc_pos_control/CMakeLists.txt | 2 +- .../mc_pos_control/mc_pos_control_params.c | 4 +-- 8 files changed, 38 insertions(+), 36 deletions(-) rename src/lib/{CollisionAvoidance => CollisionPrevention}/CMakeLists.txt (96%) rename src/lib/{CollisionAvoidance/CollisionAvoidance.cpp => CollisionPrevention/CollisionPrevention.cpp} (88%) rename src/lib/{CollisionAvoidance/CollisionAvoidance.hpp => CollisionPrevention/CollisionPrevention.hpp} (86%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index e440cf07d971..c0734c41019d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,4 +56,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(WeatherVane) -add_subdirectory(CollisionAvoidance) +add_subdirectory(CollisionPrevention) diff --git a/src/lib/CollisionAvoidance/CMakeLists.txt b/src/lib/CollisionPrevention/CMakeLists.txt similarity index 96% rename from src/lib/CollisionAvoidance/CMakeLists.txt rename to src/lib/CollisionPrevention/CMakeLists.txt index 8bf01178bbe0..b9ef18f7c361 100644 --- a/src/lib/CollisionAvoidance/CMakeLists.txt +++ b/src/lib/CollisionPrevention/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(CollisionAvoidance CollisionAvoidance.cpp) +px4_add_library(CollisionPrevention CollisionPrevention.cpp) diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp similarity index 88% rename from src/lib/CollisionAvoidance/CollisionAvoidance.cpp rename to src/lib/CollisionPrevention/CollisionPrevention.cpp index 9b712cc553b6..38e49a41a10a 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -32,31 +32,33 @@ ****************************************************************************/ /** - * @file CollisionAvoidance.cpp - * CollisionAvoidance controller. + * @file CollisionPrevention.cpp + * CollisionPrevention controller. * */ -#include "CollisionAvoidance.hpp" +#include -CollisionAvoidance::CollisionAvoidance() : +CollisionPrevention::CollisionPrevention() : ModuleParams(nullptr) { } -CollisionAvoidance::~CollisionAvoidance(){ +CollisionPrevention::~CollisionPrevention() +{ //unadvertise publishers if (_constraints_pub != nullptr) { orb_unadvertise(_constraints_pub); } + if (_mavlink_log_pub != nullptr) { orb_unadvertise(_mavlink_log_pub); } } -bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) +bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { return false; @@ -65,7 +67,7 @@ bool CollisionAvoidance::initializeSubscriptions(SubscriptionArray &subscription return true; } -void CollisionAvoidance::reset_constraints() +void CollisionPrevention::reset_constraints() { _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction @@ -80,7 +82,7 @@ void CollisionAvoidance::reset_constraints() } -void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) +void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) { collision_constraints_s constraints; /**< collision constraints message */ @@ -107,10 +109,10 @@ void CollisionAvoidance::publish_constraints(const Vector2f &original_setpoint, } -void CollisionAvoidance::update() +void CollisionPrevention::update() { - // activate/deactivate the collision avoidance based on MPC_COL_AVOID parameter - if (collision_avoidance_enabled()) { + // activate/deactivate the collision prevention based on MPC_COL_PREV parameter + if (collision_prevention_enabled()) { activate(); } else { @@ -118,7 +120,7 @@ void CollisionAvoidance::update() } } -void CollisionAvoidance::update_range_constraints() +void CollisionPrevention::update_range_constraints() { obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get(); @@ -132,8 +134,8 @@ void CollisionAvoidance::update_range_constraints() float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters float angle = i * obstacle_distance.increment * (M_PI / 180.0); //calculate normalized velocity reductions - float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * cos(angle); - float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_AVOID_D.get()) * sin(angle); + float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle); + float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle); if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; } @@ -151,7 +153,7 @@ void CollisionAvoidance::update_range_constraints() } } -void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float max_speed) +void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { update(); reset_constraints(); @@ -191,7 +193,7 @@ void CollisionAvoidance::modifySetpoint(Vector2f &original_setpoint, const float new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1)); new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1)); - //warn user if collision avoidance starts to interfere + //warn user if collision prevention starts to interfere bool currently_interfering = (new_setpoint(0) < 0.95f * original_setpoint(0) || new_setpoint(0) > 1.05f * original_setpoint(0) || new_setpoint(1) < 0.95f * original_setpoint(1) || new_setpoint(1) > 1.05f * original_setpoint(1)); diff --git a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp similarity index 86% rename from src/lib/CollisionAvoidance/CollisionAvoidance.hpp rename to src/lib/CollisionPrevention/CollisionPrevention.hpp index 66a72d75d4d3..ec82401bd954 100644 --- a/src/lib/CollisionAvoidance/CollisionAvoidance.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -32,10 +32,10 @@ ****************************************************************************/ /** - * @file CollisionAvoidance.hpp + * @file CollisionPrevention.hpp * @author Tanja Baumann * - * CollisionAvoidance controller. + * CollisionPrevention controller. * */ @@ -58,12 +58,12 @@ using uORB::Publication; using namespace matrix; using namespace time_literals; -class CollisionAvoidance : public ModuleParams +class CollisionPrevention : public ModuleParams { public: - CollisionAvoidance(); + CollisionPrevention(); - ~CollisionAvoidance(); + ~CollisionPrevention(); /** * Initialize the uORB subscriptions using an array @@ -77,7 +77,7 @@ class CollisionAvoidance : public ModuleParams bool is_active() {return _is_active;} - bool collision_avoidance_enabled() { return MPC_COL_AVOID.get(); } + bool collision_prevention_enabled() { return MPC_COL_PREV.get(); } void update(); @@ -92,7 +92,7 @@ class CollisionAvoidance : public ModuleParams private: bool _is_active = true; - bool _interfering = false; /**< states if the collision avoidance interferes with the user input */ + bool _interfering = false; /**< states if the collision prevention interferes with the user input */ orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ orb_advert_t _mavlink_log_pub = nullptr; /**< Mavlink log uORB handle */ @@ -110,8 +110,8 @@ class CollisionAvoidance : public ModuleParams Vector2f _move_constraints_y; DEFINE_PARAMETERS( - (ParamInt) MPC_COL_AVOID, /**< use range sensor measurements to avoid collision */ - (ParamFloat) MPC_COL_AVOID_D /**< collision avoidance keep minimum distance */ + (ParamInt) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */ + (ParamFloat) MPC_COL_PREV_D /**< collision prevention keep minimum distance */ ) }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 74457e8e929d..a6d18786dcbc 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -47,7 +47,7 @@ bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscr return false; } - if (!_collision_avoidance.initializeSubscriptions(subscription_array)) { + if (!_collision_prevention.initializeSubscriptions(subscription_array)) { return false; } @@ -127,9 +127,9 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); - // collision avoidance - if (_collision_avoidance.is_active()) { - _collision_avoidance.modifySetpoint(vel_sp_xy, _constraints.speed_xy); + // collision prevention + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(vel_sp_xy, _constraints.speed_xy); } _velocity_setpoint(0) = vel_sp_xy(0); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 59da9ee05745..f32393a18f33 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -40,8 +40,8 @@ #pragma once +#include #include "FlightTaskManualAltitude.hpp" -#include class FlightTaskManualPosition : public FlightTaskManualAltitude { @@ -77,5 +77,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionAvoidance _collision_avoidance; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 247a266a6c4a..f649cecbef69 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -45,5 +45,5 @@ px4_add_module( git_ecl ecl_geo WeatherVane - CollisionAvoidance + CollisionPrevention ) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index a2e1d045429c..f418ac540383 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -744,7 +744,7 @@ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); * @boolean * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); +PARAM_DEFINE_INT32(MPC_COL_PREV, 0); /** * Minimum distance the vehicle should keep to all obstacles @@ -756,4 +756,4 @@ PARAM_DEFINE_INT32(MPC_COL_AVOID, 0); * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_AVOID_D, 4.0f); +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); From f5369db2458cad9f0916d7b5ec213ce4accb24aa Mon Sep 17 00:00:00 2001 From: baumanta Date: Thu, 15 Nov 2018 14:43:04 +0100 Subject: [PATCH 0062/1046] Put parameters inside library. Get rid of unnecessary activation logic --- .../CollisionPrevention.cpp | 13 ---- .../CollisionPrevention.hpp | 8 +-- .../collisionprevention_params.c | 60 +++++++++++++++++++ .../mc_pos_control/mc_pos_control_params.c | 19 ------ 4 files changed, 61 insertions(+), 39 deletions(-) create mode 100644 src/lib/CollisionPrevention/collisionprevention_params.c diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 38e49a41a10a..98cf2ec2f871 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -106,18 +106,6 @@ void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, } else { _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); } - -} - -void CollisionPrevention::update() -{ - // activate/deactivate the collision prevention based on MPC_COL_PREV parameter - if (collision_prevention_enabled()) { - activate(); - - } else { - deactivate(); - } } void CollisionPrevention::update_range_constraints() @@ -155,7 +143,6 @@ void CollisionPrevention::update_range_constraints() void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed) { - update(); reset_constraints(); //calculate movement constraints based on range data diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index ec82401bd954..11561c3bb502 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -71,13 +71,7 @@ class CollisionPrevention : public ModuleParams */ bool initializeSubscriptions(SubscriptionArray &subscription_array); - void activate() {_is_active = true;} - - void deactivate() {_is_active = false;} - - bool is_active() {return _is_active;} - - bool collision_prevention_enabled() { return MPC_COL_PREV.get(); } + bool is_active() {return MPC_COL_PREV.get();} void update(); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c new file mode 100644 index 000000000000..a518571f3927 --- /dev/null +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file collisionprevention_params.c + * + * Parameters defined by the collisionprevention lib. + * + * @author Tanja Baumann + */ + +/** + * Flag to enable the use of a MAVLink range sensor for collision avoidance. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_COL_PREV, 0); + +/** + * Minimum distance the vehicle should keep to all obstacles + * + * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). + * + * @min 0 + * @max 15 + * @unit meters + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index f418ac540383..f86eb782061c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -738,22 +738,3 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); -/** - * Flag to enable the use of a MAVLink range sensor for collision avoidance. - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_COL_PREV, 0); - -/** - * Minimum distance the vehicle should keep to all obstacles - * - * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). - * - * @min 0 - * @max 15 - * @unit meters - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); From efa7e0ba7783cb18a6690d2c38ad63f6af9b15d2 Mon Sep 17 00:00:00 2001 From: baumanta Date: Mon, 19 Nov 2018 08:46:58 +0100 Subject: [PATCH 0063/1046] clean up --- .../CollisionPrevention.cpp | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 98cf2ec2f871..8bd340987de3 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -70,16 +70,11 @@ bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscriptio void CollisionPrevention::reset_constraints() { - _move_constraints_x_normalized(0) = 0.0f; //normalized constraint in negative x-direction - _move_constraints_x_normalized(1) = 0.0f; //normalized constraint in positive x-direction - _move_constraints_y_normalized(0) = 0.0f; //normalized constraint in negative y-direction - _move_constraints_y_normalized(1) = 0.0f; //normalized constraint in positive y-direction - - _move_constraints_x(0) = 0.0f; //constraint in negative x-direction - _move_constraints_x(1) = 0.0f; //constraint in positive x-direction - _move_constraints_y(0) = 0.0f; //constraint in negative y-direction - _move_constraints_y(1) = 0.0f; //constraint in positive y-direction + _move_constraints_x_normalized.zero(); //normalized constraint in x-direction + _move_constraints_y_normalized.zero(); //normalized constraint in y-direction + _move_constraints_x.zero(); //constraint in x-direction + _move_constraints_y.zero(); //constraint in y-direction } void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) @@ -164,10 +159,8 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } //scale the velocity reductions with the maximum possible velocity along the respective axis - _move_constraints_x(0) *= v_max_x; - _move_constraints_x(1) *= v_max_x; - _move_constraints_y(0) *= v_max_y; - _move_constraints_y(1) *= v_max_y; + _move_constraints_x *= v_max_x; + _move_constraints_y *= v_max_y; //apply the velocity reductions to form velocity limits _move_constraints_x(0) = v_max_x - _move_constraints_x(0); From 053494c5354a4d55bad9824f068c7da3f5edfb8d Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 20 Nov 2018 12:31:52 +0100 Subject: [PATCH 0064/1046] use only one parameter and enable parameter change in flight. clean up code --- .../CollisionPrevention.cpp | 19 ++++++---- .../CollisionPrevention.hpp | 36 +++++++++---------- .../collisionprevention_params.c | 14 ++------ .../FlightTaskManualPosition.cpp | 5 +++ .../FlightTaskManualPosition.hpp | 2 +- 5 files changed, 37 insertions(+), 39 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 8bd340987de3..76a32e1c6f0a 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -38,10 +38,12 @@ */ #include +using namespace matrix; +using namespace time_literals; -CollisionPrevention::CollisionPrevention() : - ModuleParams(nullptr) +CollisionPrevention::CollisionPrevention(ModuleParams *parent) : + ModuleParams(parent) { } @@ -105,17 +107,20 @@ void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, void CollisionPrevention::update_range_constraints() { - obstacle_distance_s obstacle_distance = _sub_obstacle_distance->get(); + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { float max_detection_distance = obstacle_distance.max_distance / 100.0f; //convert to meters - for (int i = 0; i < 72; i++) { + int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]); + + for (int i = 0; i < distances_array_size; i++) { //determine if distance bin is valid and contains a valid distance measurement if (obstacle_distance.distances[i] < obstacle_distance.max_distance && obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) { float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters - float angle = i * obstacle_distance.increment * (M_PI / 180.0); + float angle = math::radians((float)i * obstacle_distance.increment); + //calculate normalized velocity reductions float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle); float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle); @@ -146,7 +151,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa _move_constraints_y = _move_constraints_y_normalized; // calculate the maximum velocity along x,y axis when moving in the demanded direction - float vel_mag = sqrt(original_setpoint(0) * original_setpoint(0) + original_setpoint(1) * original_setpoint(1)); + float vel_mag = original_setpoint.norm(); float v_max_x, v_max_y; if (vel_mag > 0.0f) { @@ -169,7 +174,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa _move_constraints_y(1) = v_max_y - _move_constraints_y(1); //constrain the velocity setpoint to respect the velocity limits - Vector2f new_setpoint = original_setpoint; + Vector2f new_setpoint; new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1)); new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1)); diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 11561c3bb502..527655611de8 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -55,13 +55,10 @@ using uORB::Publication; #include #include -using namespace matrix; -using namespace time_literals; - class CollisionPrevention : public ModuleParams { public: - CollisionPrevention(); + CollisionPrevention(ModuleParams *parent); ~CollisionPrevention(); @@ -71,17 +68,9 @@ class CollisionPrevention : public ModuleParams */ bool initializeSubscriptions(SubscriptionArray &subscription_array); - bool is_active() {return MPC_COL_PREV.get();} - - void update(); - - void update_range_constraints(); - - void reset_constraints(); - - void publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint); + bool is_active() {return MPC_COL_PREV_D.get() > 0 ;} - void modifySetpoint(Vector2f &original_setpoint, const float max_speed); + void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed); private: @@ -94,18 +83,25 @@ class CollisionPrevention : public ModuleParams uORB::Subscription *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; - static constexpr uint64_t MESSAGE_THROTTLE_US = 5_s; + static constexpr uint64_t MESSAGE_THROTTLE_US = 5000000; hrt_abstime _last_message; - Vector2f _move_constraints_x_normalized; - Vector2f _move_constraints_y_normalized; - Vector2f _move_constraints_x; - Vector2f _move_constraints_y; + matrix::Vector2f _move_constraints_x_normalized; + matrix::Vector2f _move_constraints_y_normalized; + matrix::Vector2f _move_constraints_x; + matrix::Vector2f _move_constraints_y; DEFINE_PARAMETERS( - (ParamInt) MPC_COL_PREV, /**< use range sensor measurements to prevent collision */ (ParamFloat) MPC_COL_PREV_D /**< collision prevention keep minimum distance */ ) + void update(); + + void update_range_constraints(); + + void reset_constraints(); + + void publish_constraints(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + }; diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index a518571f3927..1b4175396b6a 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -39,22 +39,14 @@ * @author Tanja Baumann */ -/** - * Flag to enable the use of a MAVLink range sensor for collision avoidance. - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_COL_PREV, 0); - /** * Minimum distance the vehicle should keep to all obstacles * - * Only used in Position mode when collision avoidance is active (see MPC_COL_AVOID). + * Only used in Position mode. Collision avoidace is disable by setting this parameter to a negative value * - * @min 0 + * @min -1 * @max 15 * @unit meters * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, 4.0f); +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index a6d18786dcbc..6b2ec40431d8 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,6 +41,11 @@ using namespace matrix; +FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) +{ + +} + bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index f32393a18f33..031c5cb1a423 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -46,7 +46,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude { public: - FlightTaskManualPosition() = default; + FlightTaskManualPosition(); virtual ~FlightTaskManualPosition() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; From 1e17a86a3961613d443587abf98e097c273bd0a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 20 Nov 2018 14:14:13 +0100 Subject: [PATCH 0065/1046] collisionprevention: fix typo in param description & minor code style --- src/lib/CollisionPrevention/CollisionPrevention.hpp | 4 +--- src/lib/CollisionPrevention/collisionprevention_params.c | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 527655611de8..3bea60daf361 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -49,8 +49,6 @@ #include #include #include -#include -using uORB::Publication; #include #include #include @@ -68,7 +66,7 @@ class CollisionPrevention : public ModuleParams */ bool initializeSubscriptions(SubscriptionArray &subscription_array); - bool is_active() {return MPC_COL_PREV_D.get() > 0 ;} + bool is_active() { return MPC_COL_PREV_D.get() > 0; } void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed); diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index 1b4175396b6a..030f943a6540 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -42,7 +42,7 @@ /** * Minimum distance the vehicle should keep to all obstacles * - * Only used in Position mode. Collision avoidace is disable by setting this parameter to a negative value + * Only used in Position mode. Collision avoidace is disabled by setting this parameter to a negative value * * @min -1 * @max 15 From 177d14c8eaf66aa5b712210975e6794b31c9558e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Nov 2018 14:27:44 +0100 Subject: [PATCH 0066/1046] px_romfs_pruner.py: remove deprecation warning The open flag `U` causes a deprecation warning starting with Python 3.4. The option to open all kinds of newlines as `\n` has been replaced with the argument `newline=None`. However, this argument is not available for Python 2 unless we use `io.open` instead of `open`. --- Tools/px_romfs_pruner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 1b2518719f12..7fc45fb21260 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -49,6 +49,7 @@ import argparse import re import os +import io def main(): @@ -88,7 +89,7 @@ def main(): # read file line by line pruned_content = "" board_excluded = False - with open(file_path, "rU") as f: + with io.open(file_path, "r", newline=None) as f: for line in f: if re.search(r'\b{0} exclude\b'.format(args.board),line): board_excluded = True; From 62188f6151c513a9e62883d98b7d10aaf811f802 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Nov 2018 14:34:05 +0100 Subject: [PATCH 0067/1046] px_romfs_pruner.py: PEP8 and comments fixes --- Tools/px_romfs_pruner.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 7fc45fb21260..82e7c452815a 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2014-2016 PX4 Development Team. All rights reserved. +# Copyright (C) 2014-2018 PX4 Development Team. All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,10 +35,10 @@ """ px_romfs_pruner.py: -Try to keep size of ROMFS minimal. +Try to keep size of the ROMFS minimal. This script goes through the temporarily copied ROMFS data and deletes all -comments, empty lines and leading whitespace. +comments, empty lines and unnecessary whitespace. It also deletes hidden files such as auto-saved backups that a text editor might have left in the tree. @@ -91,12 +91,14 @@ def main(): board_excluded = False with io.open(file_path, "r", newline=None) as f: for line in f: - if re.search(r'\b{0} exclude\b'.format(args.board),line): - board_excluded = True; + if re.search(r'\b{0} exclude\b'.format(args.board), line): + board_excluded = True # handle mixer files differently than startup files if file_path.endswith(".mix"): - if line.startswith(("Z:", "M:", "R: ", "O:", "S:", "H:", "T:", "P:")): - # reduce multiple consecutive spaces into a single space + if line.startswith(("Z:", "M:", "R: ", "O:", "S:", + "H:", "T:", "P:")): + # reduce multiple consecutive spaces into a + # single space line_reduced = re.sub(' +', ' ', line) pruned_content += line_reduced else: From 560a9b45edd67749141e7072fbc89c510b3ecfde Mon Sep 17 00:00:00 2001 From: Eric Babyak Date: Tue, 20 Nov 2018 10:19:50 -0500 Subject: [PATCH 0068/1046] Fix for initializing Landing Target Estimator in SITL --- ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock index ae880fdbc5d9..80105badf23a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock +++ b/ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock @@ -13,3 +13,6 @@ then param set LTEST_MODE 1 param set PLD_HACC_RAD 0.1 fi + +# Start up Landing Target Estimator module +landing_target_estimator start From 68bd87887713c94ac8c1c6c616eec6d9e2760f98 Mon Sep 17 00:00:00 2001 From: PX4 Build Bot Date: Wed, 21 Nov 2018 00:37:52 +0000 Subject: [PATCH 0069/1046] Update submodule sitl_gazebo to latest Wed Nov 21 00:37:52 UTC 2018 - sitl_gazebo in PX4/Firmware (560a9b45edd67749141e7072fbc89c510b3ecfde): https://github.com/PX4/sitl_gazebo/commit/3d80f63562c24c1537e1f8423ce649c99ebc15ea - sitl_gazebo current upstream: https://github.com/PX4/sitl_gazebo/commit/2adc86b5bf84543091470966caf33c3b4f3ee666 - Changes: https://github.com/PX4/sitl_gazebo/compare/3d80f63562c24c1537e1f8423ce649c99ebc15ea...2adc86b5bf84543091470966caf33c3b4f3ee666 2adc86b 2018-11-14 Elia Tarasov - add max value for wind and wind gust means fb20a36 2018-11-14 Elia Tarasov - add normal gaussian distribution to wind gust force and direction bd5ce12 2018-11-14 Elia Tarasov - add normal gaussian distribution to wind force and direction 913c094 2018-11-07 Jacob Schloss - reverse order of test 7d08ef5 2018-11-07 Jacob Schloss - fall back to only enabling up to c++14 on old cmake 0fd93b0 2018-11-07 Jacob Schloss - set CMAKE_CXX_STANDARD so cmake does not append a defualt language standard --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 3d80f63562c2..2adc86b5bf84 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 3d80f63562c24c1537e1f8423ce649c99ebc15ea +Subproject commit 2adc86b5bf84543091470966caf33c3b4f3ee666 From 528b794841432881ecf9cb3b350b992e943b5c7a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Nov 2018 20:34:27 -0500 Subject: [PATCH 0070/1046] Update submodule mavlink v2.0 to latest Wed Nov 21 00:38:04 UTC 2018 - mavlink v2.0 in PX4/Firmware (14b8108caabb99d1bad2daef31ac51fc061b3691): https://github.com/mavlink/c_library_v2/commit/408a7bedf3074bcae265ea0ce8ec5bb3519ed272 - mavlink v2.0 current upstream: https://github.com/mavlink/c_library_v2/commit/6203e2b64395cfa416b9321f633bc3256a79f722 - Changes: https://github.com/mavlink/c_library_v2/compare/408a7bedf3074bcae265ea0ce8ec5bb3519ed272...6203e2b64395cfa416b9321f633bc3256a79f722 --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 408a7bedf307..6203e2b64395 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 408a7bedf3074bcae265ea0ce8ec5bb3519ed272 +Subproject commit 6203e2b64395cfa416b9321f633bc3256a79f722 From 7d54913f80d23a58990a39cfb758eb1f55dd32f2 Mon Sep 17 00:00:00 2001 From: PX4 Build Bot Date: Wed, 21 Nov 2018 00:38:27 +0000 Subject: [PATCH 0071/1046] Update submodule matrix to latest Wed Nov 21 00:38:27 UTC 2018 - matrix in PX4/Firmware (4005de7b9600db8a310d52f9bf7a47a0019868d8): https://github.com/PX4/Matrix/commit/0d3bff5e006cfaa358b51e3a6d11984e3782a90e - matrix current upstream: https://github.com/PX4/Matrix/commit/9c0acfba36a928b7b002eea3a9dca54a5a0c1250 - Changes: https://github.com/PX4/Matrix/compare/0d3bff5e006cfaa358b51e3a6d11984e3782a90e...9c0acfba36a928b7b002eea3a9dca54a5a0c1250 9c0acfb 2018-11-20 Matthias Grob - Matrix: remove unsafe copyToRaw method --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 0d3bff5e006c..9c0acfba36a9 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 0d3bff5e006cfaa358b51e3a6d11984e3782a90e +Subproject commit 9c0acfba36a928b7b002eea3a9dca54a5a0c1250 From 8566b6b53e95ad8f4b16acb9e48308ecf7ef5040 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Nov 2018 00:00:08 -0500 Subject: [PATCH 0072/1046] AV-X increase logger buffer --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 5349ce700d4d..364eb965ef20 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -17,7 +17,7 @@ fi # Begin Setup for board specific configurations. # ############################################################################### -if ver hwcmp PX4FMU_V5 +if ver hwcmp PX4FMU_V5 AV_X_V1 then set LOGGER_BUF 64 fi From 20b3adebf412c65ceddb8c4f6faaa92dd05bbe7e Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 19 Nov 2018 17:51:05 +0000 Subject: [PATCH 0073/1046] attitude_estimator_q_main: remove epsilon comparison; fix logic --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 2c9aa01c0640..f72f11e694b6 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -350,10 +350,10 @@ void AttitudeEstimatorQ::task_main() if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_odom_sub, &vision) == PX4_OK) { // validation check for vision attitude data bool vision_att_valid = PX4_ISFINITE(vision.q[0]) - && (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? fabsf(sqrtf(fmaxf( + && (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf( vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE], fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE], - vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) - _eo_max_std_dev) < FLT_EPSILON : true); + vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true); if (vision_att_valid) { Dcmf Rvis = Quatf(vision.q); @@ -382,10 +382,10 @@ void AttitudeEstimatorQ::task_main() if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_odom_sub, &mocap) == PX4_OK) { // validation check for mocap attitude data bool mocap_att_valid = PX4_ISFINITE(mocap.q[0]) - && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? fabsf(sqrtf(fmaxf( + && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf( mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE], fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE], - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) - _eo_max_std_dev) < FLT_EPSILON : true); + mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true); if (mocap_att_valid) { Dcmf Rmoc = Quatf(mocap.q); From adc81aaab517393ac7dee0f8a600c812afa09b9c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 19 Nov 2018 17:51:45 +0000 Subject: [PATCH 0074/1046] position_estimator_inav: fix covariance check logic --- .../position_estimator_inav_main.cpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 17b52294021c..c7ff8c6e9a4e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -792,18 +792,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; - vision_xy_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE], - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true; - vision_z_valid = PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev : - true; - vision_vxy_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf( + vision_xy_valid = PX4_ISFINITE(visual_odom.x) + && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( + visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE], + visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); + vision_z_valid = PX4_ISFINITE(visual_odom.z) + && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? + visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); + vision_vxy_valid = PX4_ISFINITE(visual_odom.vx) + && PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf( fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE], - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) > ev_max_std_dev : true; - vision_vz_valid = PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] > - ep_max_std_dev : true; + visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) <= ev_max_std_dev : true; + vision_vz_valid = PX4_ISFINITE(visual_odom.vz) + && (PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? + visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] <= ep_max_std_dev : true); /* reset position estimate on first vision update */ if (vision_xy_valid) { @@ -917,12 +919,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap); - mocap_xy_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE], - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) > ep_max_std_dev : true) ? false : true; - mocap_z_valid = (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] > ep_max_std_dev : true) ? false : - true; + mocap_xy_valid = PX4_ISFINITE(mocap.x) + && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( + mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE], + mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); + mocap_z_valid = PX4_ISFINITE(mocap.z) + && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? + mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); if (!params.disable_mocap) { /* reset position estimate on first mocap update */ From dcdddacdb2bc0e5fc67fe4d7131e7253a56f50b1 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 20 Nov 2018 21:46:11 +0000 Subject: [PATCH 0075/1046] init.d-posix: vision: set the appropriate parameters --- ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow | 2 ++ ROMFS/px4fmu_common/init.d-posix/1013_iris_vision | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow index e042336d1b4d..8a89b65326f5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow @@ -9,11 +9,13 @@ sh /etc/init.d-posix/10016_iris if [ $AUTOCNF = yes ] then + # EKF2 param set EKF2_AID_MASK 2 param set EKF2_EV_DELAY 5 param set EKF2_EVP_NOISE 0.05 param set EKF2_EVA_NOISE 0.05 + # INAV param set INAV_LIDAR_EST 1 param set INAV_W_XY_FLOW 1.0 param set INAV_W_XY_GPS_P 0.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision index 27bd0371cc51..aa225de2caf3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision @@ -9,9 +9,11 @@ sh /etc/init.d-posix/1010_iris_opt_flow if [ $AUTOCNF = yes ] then - param set EKF2_AID_MASK 8 + # EKF2: Vision position and heading + param set EKF2_AID_MASK 24 param set EKF2_EV_DELAY 5 - # LPE: Vision + baro + # LPE: Vision + baro | AEQ: External heading set to use vision input param set LPE_FUSION 132 + param set ATT_EXT_HDG_M 1 fi From e3af000fdba9af82e412409bdd1e64729ea3897f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 20 Nov 2018 22:04:35 +0000 Subject: [PATCH 0076/1046] init.d-posix: make 1013_iris_vision independent of 1010_iris_opt_flow --- ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow | 1 - .../init.d-posix/1010_iris_opt_flow.post | 1 - ROMFS/px4fmu_common/init.d-posix/1013_iris_vision | 13 +++++++++++-- .../init.d-posix/1013_iris_vision.post | 3 +++ 4 files changed, 14 insertions(+), 4 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow index 8a89b65326f5..4093c74aa70b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow @@ -11,7 +11,6 @@ if [ $AUTOCNF = yes ] then # EKF2 param set EKF2_AID_MASK 2 - param set EKF2_EV_DELAY 5 param set EKF2_EVP_NOISE 0.05 param set EKF2_EVA_NOISE 0.05 diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post index 7806a8f2d037..868f402651e5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post +++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow.post @@ -1,4 +1,3 @@ # shellcheck disable=SC2154 -mavlink stream -r 10 -s DISTANCE_SENSOR -u $udp_gcs_port_local mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision index aa225de2caf3..8f0cd8c458cd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision @@ -5,7 +5,7 @@ # @type Quadrotor Wide # -sh /etc/init.d-posix/1010_iris_opt_flow +sh /etc/init.d-posix/10016_iris if [ $AUTOCNF = yes ] then @@ -13,7 +13,16 @@ then param set EKF2_AID_MASK 24 param set EKF2_EV_DELAY 5 - # LPE: Vision + baro | AEQ: External heading set to use vision input + # INAV: trust more on the vision input + param set INAV_W_XY_VIS_P 9.0 + param set INAV_W_Z_VIS_P 7.0 + param set INAV_W_XY_GPS_P 0.0 + param set INAV_W_XY_GPS_V 0.0 + param set INAV_W_Z_GPS_P 0.0 + + # LPE: Vision + baro param set LPE_FUSION 132 + + # AEQ: External heading set to use vision input param set ATT_EXT_HDG_M 1 fi diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post new file mode 100644 index 000000000000..868f402651e5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision.post @@ -0,0 +1,3 @@ + +# shellcheck disable=SC2154 +mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local From be16baaa3c315c608be7a26d4188eacebd71566d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 21 Nov 2018 13:58:22 +0100 Subject: [PATCH 0077/1046] sitl_tests: exclude uorb test from Windows build Because it was always failing from the beginning on and we want to make sure no other tests break in the meantime by running the currently passing tests also on Windows CI. --- platforms/posix/cmake/sitl_tests.cmake | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index f8b29ceec0fb..9990fc2a9adf 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -43,6 +43,14 @@ if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") ) endif() +if (CMAKE_SYSTEM_NAME STREQUAL "CYGWIN") + list(REMOVE_ITEM tests + uorb + ) +endif() + +message("${tests}") + foreach(test_name ${tests}) configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated) From 919a508935b76890bf44801bdd081a162ddc2a62 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 21 Nov 2018 13:59:27 +0100 Subject: [PATCH 0078/1046] appveyor: run make test also on Windows CI --- appveyor.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/appveyor.yml b/appveyor.yml index 73f093eef59b..395c8e695b0e 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -40,6 +40,8 @@ build_script: - call bash --login -c "cd $repopath && make posix" # make pixracer to check NuttX build - call bash --login -c "cd $repopath && make px4fmu-v4_default" +# run tests +- call bash --login -c "cd $repopath && make tests" # Note: using bash --login is important # because otherwise certain things (like python; import numpy) do not work From 75c1396ed7a4ebf01e946d85d3abef417fedaa0f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Nov 2018 10:56:31 -0500 Subject: [PATCH 0079/1046] cmake sitl_tests silence debug message --- platforms/posix/cmake/sitl_tests.cmake | 2 -- 1 file changed, 2 deletions(-) diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 9990fc2a9adf..0b36456e564c 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -49,8 +49,6 @@ if (CMAKE_SYSTEM_NAME STREQUAL "CYGWIN") ) endif() -message("${tests}") - foreach(test_name ${tests}) configure_file(${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_template.in ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_${test_name}_generated) From 90bfdb6f0a2832268baa4d91f88a9e3130a335ff Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 22 Nov 2018 02:32:40 +0100 Subject: [PATCH 0080/1046] VTOL rate control architecture improvements (#10819) * attitude and rate setpoint message: use 3D array for thrust demand * FixedWingAttitudeControl: rework airspeed scaling * move airspeed and scaling calculation into separate method * if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed --- Tools/uorb_graph/create.py | 2 - msg/vehicle_attitude_setpoint.msg | 4 +- msg/vehicle_rates_setpoint.msg | 5 +- src/examples/rover_steering_control/main.cpp | 2 +- .../FixedwingAttitudeControl.cpp | 222 +++++++++++------- .../FixedwingAttitudeControl.hpp | 17 +- .../FixedwingPositionControl.cpp | 14 +- .../GroundRoverAttitudeControl.cpp | 2 +- .../GroundRoverPositionControl.cpp | 8 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +- src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 24 +- .../mc_pos_control/Utility/ControlMath.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/vtol_att_control/standard.cpp | 41 ++-- src/modules/vtol_att_control/standard.h | 22 +- src/modules/vtol_att_control/tailsitter.cpp | 195 +++++++-------- src/modules/vtol_att_control/tailsitter.h | 31 ++- src/modules/vtol_att_control/tiltrotor.cpp | 21 +- src/modules/vtol_att_control/tiltrotor.h | 25 +- .../vtol_att_control/tiltrotor_params.c | 25 -- .../vtol_att_control_main.cpp | 57 +---- .../vtol_att_control/vtol_att_control_main.h | 11 +- .../vtol_att_control_params.c | 36 +++ src/modules/vtol_att_control/vtol_type.h | 2 + src/systemcmds/tests/test_controlmath.cpp | 6 +- 28 files changed, 382 insertions(+), 409 deletions(-) diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index d5fb89b52a8f..7f4dd2c67abd 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -246,10 +246,8 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'), ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), - ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index da0b01b97f24..9b5c5db01594 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -12,7 +12,9 @@ float32 yaw_sp_move_rate # rad/s (commanded by user) float32[4] q_d # Desired quaternion for quaternion control bool q_d_valid # Set to true if quaternion vector is valid -float32 thrust # Thrust in Newton the power system should generate +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg index 14753f4be8c8..ae7a3c78223d 100644 --- a/msg/vehicle_rates_setpoint.msg +++ b/msg/vehicle_rates_setpoint.msg @@ -3,6 +3,7 @@ uint64 timestamp # time since system start (microseconds) float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame float32 yaw # body angular rates in NED frame -float32 thrust # thrust normalized to 0..1 -# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index d72f1cd9cb2d..723a371b0514 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -182,7 +182,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st actuators->control[2] = yaw_err * pp.yaw_p; /* copy throttle */ - actuators->control[3] = att_sp->thrust; + actuators->control[3] = att_sp->thrust_body[0]; actuators->timestamp = hrt_absolute_time(); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 2511eef767c6..acc3e5bb9f15 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -48,6 +48,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")) { + // check if VTOL first + vehicle_status_poll(); + _parameter_handles.p_tc = param_find("FW_P_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -116,13 +119,43 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : // initialize to invalid VTOL type _parameters.vtol_type = -1; + _parameters.vtol_airspeed_rule = 0; /* fetch initial parameter values */ parameters_update(); + + // set initial maximum body rate setpoints + _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); + _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); + _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); + _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); + + // subscriptions + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + _rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); } FixedwingAttitudeControl::~FixedwingAttitudeControl() { + orb_unsubscribe(_att_sub); + orb_unsubscribe(_att_sp_sub); + orb_unsubscribe(_vcontrol_mode_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_manual_sub); + orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_vehicle_status_sub); + orb_unsubscribe(_vehicle_land_detected_sub); + orb_unsubscribe(_battery_status_sub); + orb_unsubscribe(_rates_sp_sub); + perf_free(_loop_perf); perf_free(_nonfinite_input_perf); perf_free(_nonfinite_output_perf); @@ -131,7 +164,6 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() int FixedwingAttitudeControl::parameters_update() { - int32_t tmp = 0; param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -209,6 +241,7 @@ FixedwingAttitudeControl::parameters_update() if (_vehicle_status.is_vtol) { param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + param_get(_parameter_handles.vtol_airspeed_rule, &_parameters.vtol_airspeed_rule); } param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); @@ -249,13 +282,12 @@ FixedwingAttitudeControl::parameters_update() void FixedwingAttitudeControl::vehicle_control_mode_poll() { - bool vcontrol_mode_updated; + bool updated = false; /* Check if vehicle control mode has changed */ - orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - - if (vcontrol_mode_updated) { + orb_check(_vcontrol_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -263,8 +295,8 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() void FixedwingAttitudeControl::vehicle_manual_poll() { - // only update manual if in a manual mode - if (_vcontrol_mode.flag_control_manual_enabled) { + + if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_wing) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) { @@ -287,7 +319,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); _att_sp.yaw_body = 0.0f; - _att_sp.thrust = _manual.z; + _att_sp.thrust_body[0] = _manual.z; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -310,15 +342,15 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad; _rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad; _rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad; - _rates_sp.thrust = _manual.z; + _rates_sp.thrust_body[0] = _manual.z; if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ - orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - } else if (_rates_sp_id) { + } else { /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } } else { @@ -335,14 +367,36 @@ FixedwingAttitudeControl::vehicle_manual_poll() } void -FixedwingAttitudeControl::vehicle_setpoint_poll() +FixedwingAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated = false; + orb_check(_att_sp_sub, &updated); + + if (updated) { + if (orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp) == PX4_OK) { + _rates_sp.thrust_body[0] = _att_sp.thrust_body[0]; + _rates_sp.thrust_body[1] = _att_sp.thrust_body[1]; + _rates_sp.thrust_body[2] = _att_sp.thrust_body[2]; + } + } +} - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); +void +FixedwingAttitudeControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated = false; + orb_check(_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp); + + if (_parameters.vtol_type == vtol_type::TAILSITTER) { + float tmp = _rates_sp.roll; + _rates_sp.roll = -_rates_sp.yaw; + _rates_sp.yaw = tmp; + } } } @@ -368,19 +422,28 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + // if VTOL and not in fixed wing mode we should only control body-rates which are published + // by the multicoper attitude controller. Therefore, modify the control mode to achieve rate + // control only + if (_vehicle_status.is_vtol) { + if (_vehicle_status.is_rotary_wing) { + _vcontrol_mode.flag_control_attitude_enabled = false; + _vcontrol_mode.flag_control_manual_enabled = false; + } + } + /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (!_rates_sp_id) { + if (!_actuators_id) { if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); _parameter_handles.vtol_type = param_find("VT_TYPE"); + _parameter_handles.vtol_airspeed_rule = param_find("VT_AIRSPD_RULE"); parameters_update(); } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -404,30 +467,44 @@ FixedwingAttitudeControl::vehicle_land_detected_poll() } } -void FixedwingAttitudeControl::run() +void FixedwingAttitudeControl::get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling) { - /* - * do subscriptions - */ - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s) + && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f) + && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6); - parameters_update(); + if (!_parameters.airspeed_disabled && airspeed_valid) { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s); - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - vehicle_status_poll(); - vehicle_land_detected_poll(); + } else { + // if no airspeed measurement is available out best guess is to use the trim airspeed + airspeed = _parameters.airspeed_trim; + + // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible + // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger + // than the minimum airspeed + if (_vehicle_status.is_vtol && _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { + airspeed = _parameters.airspeed_min; + } + if (!airspeed_valid) { + perf_count(_nonfinite_input_perf); + } + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + airspeed_scaling = _parameters.airspeed_trim / math::max(airspeed, _parameters.airspeed_min); +} + +void FixedwingAttitudeControl::run() +{ /* wakeup source */ px4_pollfd_struct_t fds[1]; @@ -529,7 +606,7 @@ void FixedwingAttitudeControl::run() matrix::Eulerf euler_angles(R); _airspeed_sub.update(); - vehicle_setpoint_poll(); + vehicle_attitude_setpoint_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); global_pos_poll(); @@ -560,35 +637,10 @@ void FixedwingAttitudeControl::run() /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_rates_enabled) { - /* scale around tuning airspeed */ - float airspeed; - - /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */ - const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s) - && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f) - && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6); - - if (!_parameters.airspeed_disabled && airspeed_valid) { - /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s); - - } else { - airspeed = _parameters.airspeed_trim; - - if (!airspeed_valid) { - perf_count(_nonfinite_input_perf); - } - } - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : - airspeed); + float airspeed; + float airspeed_scaling; + get_airspeed_and_scaling(airspeed, airspeed_scaling); /* Use min airspeed to calculate ground speed scaling region. * Don't scale below gspd_scaling_trim @@ -624,10 +676,6 @@ void FixedwingAttitudeControl::run() _wheel_ctrl.reset_integrator(); } - float roll_sp = _att_sp.roll_body; - float pitch_sp = _att_sp.pitch_body; - float yaw_sp = _att_sp.yaw_body; - /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; control_input.roll = euler_angles.phi(); @@ -636,9 +684,9 @@ void FixedwingAttitudeControl::run() control_input.body_x_rate = _att.rollspeed; control_input.body_y_rate = _att.pitchspeed; control_input.body_z_rate = _att.yawspeed; - control_input.roll_setpoint = roll_sp; - control_input.pitch_setpoint = pitch_sp; - control_input.yaw_setpoint = yaw_sp; + control_input.roll_setpoint = _att_sp.roll_body; + control_input.pitch_setpoint = _att_sp.pitch_body; + control_input.yaw_setpoint = _att_sp.yaw_body; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; @@ -649,7 +697,7 @@ void FixedwingAttitudeControl::run() /* reset body angular rate limits on mode change */ if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { - if (_vcontrol_mode.flag_control_attitude_enabled) { + if (_vcontrol_mode.flag_control_attitude_enabled || _vehicle_status.is_rotary_wing) { _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); @@ -693,7 +741,7 @@ void FixedwingAttitudeControl::run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { - if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { + if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude @@ -744,8 +792,8 @@ void FixedwingAttitudeControl::run() } /* throttle passed through if it is finite and if no engine failure was detected */ - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust) - && !_vehicle_status.engine_failure) ? _att_sp.thrust : 0.0f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0]) + && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f; /* scale effort by battery status */ if (_parameters.bat_scale_en && @@ -783,18 +831,19 @@ void FixedwingAttitudeControl::run() if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ - orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - } else if (_rates_sp_id) { + } else { /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } } else { - // pure rate control + vehicle_rates_setpoint_poll(); + _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); - _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); + _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; @@ -805,7 +854,8 @@ void FixedwingAttitudeControl::run() float yaw_u = _yaw_ctrl.control_bodyrate(control_input); _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? + _rates_sp.thrust_body[0] : 0.0f; } rate_ctrl_status_s rate_ctrl_status; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index b4d7e14e28bc..d9c414f78db5 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -95,6 +95,7 @@ class FixedwingAttitudeControl final : public ModuleBase 0.0f && diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 8593b807a020..4960b788e95d 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -261,7 +261,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = 0.0f; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; } else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { @@ -272,7 +272,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; - _att_sp.thrust = mission_throttle; + _att_sp.thrust_body[0] = mission_throttle; } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { @@ -284,7 +284,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; } if (was_circle_mode && !_gnd_control.circle_mode()) { @@ -299,7 +299,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = 0.0f; _att_sp.fw_control_yaw = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; /* do not publish the setpoint */ setpoint = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7817e884c80b..5ae3b873c10d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3138,7 +3138,7 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream msg.body_pitch_rate = att_rates_sp.pitch; msg.body_yaw_rate = att_rates_sp.yaw; - msg.thrust = att_sp.thrust; + msg.thrust = att_sp.thrust_body[0]; mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b062aa9f08a9..c7875f37c194 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1447,8 +1447,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.yaw_sp_move_rate = 0.0f; } + // TODO: We assume offboard is only used for multicopters which produce thrust along the + // body z axis. If we want to support fixed wing as well we need to handle it differently here, e.g. + // in that case we should assign att_sp.thrust_body[0] if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - att_sp.thrust = set_attitude_target.thrust; + att_sp.thrust_body[2] = -set_attitude_target.thrust; } if (_att_sp_pub == nullptr) { @@ -1472,7 +1475,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - rates_sp.thrust = set_attitude_target.thrust; + rates_sp.thrust_body[2] = -set_attitude_target.thrust; } if (_rates_sp_pub == nullptr) { diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index eb95da8d183f..c3dad3c7ab56 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -166,7 +166,6 @@ class MulticopterAttitudeControl : public ModuleBase orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; - orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ @@ -196,8 +195,9 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ matrix::Vector3f _rates_sp; /**< angular rates setpoint */ matrix::Vector3f _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ + matrix::Vector3f _att_control; /**< attitude control vector */ + float _thrust_sp{0.0f}; /**< thrust setpoint */ matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c4858a5db364..020096ac63c1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -273,13 +273,11 @@ MulticopterAttitudeControl::vehicle_status_poll() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_rates_sp_id == nullptr) { + if (_actuators_id == nullptr) { if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); } } @@ -508,7 +506,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ q_sp.copyTo(attitude_setpoint.q_d); attitude_setpoint.q_d_valid = true; - attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z); + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); attitude_setpoint.landing_gear = get_landing_gear_state(); attitude_setpoint.timestamp = hrt_absolute_time(); @@ -524,7 +522,9 @@ void MulticopterAttitudeControl::control_attitude() { vehicle_attitude_setpoint_poll(); - _thrust_sp = _v_att_sp.thrust; + + // physical thrust axis is the negative of body z axis + _thrust_sp = -_v_att_sp.thrust_body[2]; /* prepare yaw weight from the ratio between roll/pitch and yaw gains */ Vector3f attitude_gain = _attitude_p; @@ -607,7 +607,7 @@ Vector3f MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) { /* throttle pid attenuation factor */ - float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint); + float tpa = 1.0f - tpa_rate * (fabsf(_thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint); tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); Vector3f pidAttenuationPerAxis; @@ -731,9 +731,11 @@ MulticopterAttitudeControl::publish_rates_setpoint() _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.thrust_body[0] = 0.0f; + _v_rates_sp.thrust_body[1] = 0.0f; + _v_rates_sp.thrust_body[2] = -_thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); - orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); } void @@ -886,7 +888,7 @@ MulticopterAttitudeControl::run() bool attitude_setpoint_generated = false; - if (_v_control_mode.flag_control_attitude_enabled) { + if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) { if (attitude_updated) { // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode if (_v_control_mode.flag_control_manual_enabled && @@ -903,7 +905,7 @@ MulticopterAttitudeControl::run() } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { + if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) { if (manual_control_updated) { /* manual rates control - ACRO mode */ Vector3f man_rate_sp( @@ -921,7 +923,7 @@ MulticopterAttitudeControl::run() _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; + _thrust_sp = -_v_rates_sp.thrust_body[2]; } } } diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index f5d28d9dbe28..b3cfa8afa1d2 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -103,7 +103,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float Eulerf euler = R_sp; att_sp.roll_body = euler(0); att_sp.pitch_body = euler(1); - att_sp.thrust = thr_sp.length(); + att_sp.thrust_body[2] = -thr_sp.length(); return att_sp; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0b50c5d7b4c2..94e1380cc3e6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -827,7 +827,7 @@ MulticopterPositionControl::run() matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[2] = 0.0f; } } diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index d51e5433848b..69b37c793fbf 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -86,7 +86,7 @@ GpsFailure::on_active() att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = math::radians(_param_openlooploiter_roll.get()); att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get()); - att_sp.thrust = _param_openlooploiter_thrust.get(); + att_sp.thrust_body[0] = _param_openlooploiter_thrust.get(); Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); q.copyTo(att_sp.q_d); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 506f810f402b..7f37bc2f6935 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -46,13 +46,10 @@ #include -using matrix::wrap_pi; +using namespace matrix; Standard::Standard(VtolAttitudeControl *attc) : - VtolType(attc), - _pusher_throttle(0.0f), - _reverse_output(0.0f), - _airspeed_trans_blend_margin(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -156,8 +153,8 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed - const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed()); - const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); + const Dcmf R_to_body(Quatf(_v_att->q).inversed()); + const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); float x_vel = vel(0); @@ -261,7 +258,7 @@ void Standard::update_transition_state() // ramp up FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); - matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; @@ -277,7 +274,7 @@ void Standard::update_transition_state() // maintain FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; - matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; @@ -333,18 +330,18 @@ void Standard::update_mc_state() return; } - matrix::Dcmf R(matrix::Quatf(_v_att->q)); - matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); - matrix::Eulerf euler(R); - matrix::Eulerf euler_sp(R_sp); + const Dcmf R(Quatf(_v_att->q)); + const Dcmf R_sp(Quatf(_v_att_sp->q_d)); + const Eulerf euler(R); + const Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; // direction of desired body z axis represented in earth frame - matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. - matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); + Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2)); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); @@ -365,25 +362,25 @@ void Standard::update_mc_state() float pitch_new = 0.0f; // create corrected desired body z axis in heading frame - matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); - matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); + const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f); + Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); // rotate the vector into a new frame which is rotated in z by the desired heading // with respect to the earh frame. const float yaw_error = wrap_pi(euler_sp(2) - euler(2)); - matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); + const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); _v_att_sp->roll_body = -asinf(tilt_new(1)); - R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)); - matrix::Quatf q_sp(R_sp); + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); + _v_att_sp->q_d_valid = true; } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; - } void Standard::update_fw_state() @@ -475,5 +472,5 @@ void Standard::waiting_on_tecs() { // keep thrust from transition - _v_att_sp->thrust = _pusher_throttle; + _v_att_sp->thrust_body[0] = _pusher_throttle; }; diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 884753fa6f3f..27ada9c8e6f3 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -55,14 +55,14 @@ class Standard : public VtolType public: Standard(VtolAttitudeControl *_att_controller); - ~Standard() = default; + ~Standard() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void update_fw_state(); - virtual void update_mc_state(); - virtual void fill_actuator_outputs(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void update_mc_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; private: @@ -98,10 +98,10 @@ class Standard : public VtolType hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) } _vtol_schedule; - float _pusher_throttle; - float _reverse_output; - float _airspeed_trans_blend_margin; + float _pusher_throttle{0.0f}; + float _reverse_output{0.0f}; + float _airspeed_trans_blend_margin{0.0f}; - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index be21121bc6fc..974f1c282625 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -47,11 +47,10 @@ #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC +using namespace matrix; + Tailsitter::Tailsitter(VtolAttitudeControl *attc) : - VtolType(attc), - _thrust_transition_start(0.0f), - _yaw_transition(0.0f), - _pitch_transition_start(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -63,6 +62,7 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : _flag_was_in_trans_mode = false; _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF"); } void @@ -73,18 +73,20 @@ Tailsitter::parameters_update() /* vtol front transition phase 2 duration */ param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); _params_tailsitter.front_trans_dur_p2 = v; + + param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v); + _params_tailsitter.fw_pitch_sp_offset = math::radians(v); } void Tailsitter::update_vtol_state() { - /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - matrix::Eulerf euler = matrix::Quatf(_v_att->q); + Eulerf euler = Quatf(_v_att->q); float pitch = euler.theta(); if (!_attc->is_fixed_wing_requested()) { @@ -104,9 +106,10 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_BACK: + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; // check if we have reached pitch angle to switch to MC mode - if (pitch >= PITCH_TRANSITION_BACK) { + if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) { _vtol_schedule.flight_mode = MC_MODE; } @@ -176,89 +179,94 @@ void Tailsitter::update_transition_state() float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; if (!_flag_was_in_trans_mode) { - // save desired heading for transition and last thrust value - _yaw_transition = _v_att_sp->yaw_body; - //transition should start from current attitude instead of current setpoint - matrix::Eulerf euler = matrix::Quatf(_v_att->q); - _pitch_transition_start = euler.theta(); - _thrust_transition_start = _v_att_sp->thrust; _flag_was_in_trans_mode = true; - } - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + // calculate rotation axis for transition. + _q_trans_start = Quatf(_v_att->q); + Vector3f z = -_q_trans_start.dcm_z(); + _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + + // as heading setpoint we choose the heading given by the direction the vehicle points + float yaw_sp = atan2f(z(1), z(0)); - // create time dependant pitch angle set point + 0.2 rad overlap over the switch value - _v_att_sp->pitch_body = _pitch_transition_start - fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * - time_since_trans_start / _params->front_trans_duration; - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f, - _pitch_transition_start); + // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, + // the yaw setpoint and zero roll since we want wings level transition + _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); - // disable mc yaw control once the plane has picked up speed - if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { - _mc_yaw_weight = 0.0f; + // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the + // multirotor frame + _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0)); - } else { - _mc_yaw_weight = 1.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + // initial attitude setpoint for the transition should be with wings level + _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0); + _trans_rot_axis = -x.cross(Vector3f(0, 0, -1)); } - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; + _q_trans_sp = _q_trans_start; + } - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + // tilt angle (zero if vehicle nose points up (hover)) + float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * _q_trans_sp( + 2) + _q_trans_sp(3) * _q_trans_sp(3)); - if (!flag_idle_mc) { - flag_idle_mc = set_idle_mc(); - } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { - // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value - _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * - time_since_trans_start / _params->back_trans_duration; - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f); + const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration; - // keep yaw disabled - _mc_yaw_weight = 0.0f; + if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) { + _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, + time_since_trans_start * trans_pitch_rate)) * _q_trans_start; + } - // smoothly move control weight to MC - _mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { - } + const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration; - if (_v_control_mode->flag_control_climb_rate_enabled) { - _v_att_sp->thrust = _params->front_trans_throttle; + if (!flag_idle_mc) { + flag_idle_mc = set_idle_mc(); + } - } else { - _v_att_sp->thrust = _mc_virtual_att_sp->thrust; + if (tilt > 0.01f) { + _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, + time_since_trans_start * trans_pitch_rate)) * _q_trans_start; + } } - _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); - _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); - _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2]; - // compute desired attitude and thrust setpoint for the transition + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; _v_att_sp->timestamp = hrt_absolute_time(); - _v_att_sp->roll_body = 0.0f; - _v_att_sp->yaw_body = _yaw_transition; - matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); - q_sp.copyTo(_v_att_sp->q_d); + const Eulerf euler_sp(_q_trans_sp); + _v_att_sp->roll_body = euler_sp.phi(); + _v_att_sp->pitch_body = euler_sp.theta(); + _v_att_sp->yaw_body = euler_sp.psi(); + + _q_trans_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; } void Tailsitter::waiting_on_tecs() { // copy the last trust value from the front transition - _v_att_sp->thrust = _thrust_transition; -} - -void Tailsitter::update_mc_state() -{ - VtolType::update_mc_state(); + _v_att_sp->thrust_body[0] = _thrust_transition; } void Tailsitter::update_fw_state() { VtolType::update_fw_state(); + + // allow fw yawrate control via multirotor roll actuation. this is useful for vehicles + // which don't have a rudder to coordinate turns + if (_params->diff_thrust == 1) { + _mc_roll_weight = 1.0f; + } } /** @@ -272,67 +280,30 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->timestamp = hrt_absolute_time(); _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - switch (_vtol_mode) { - case ROTARY_WING: - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - - if (_params->elevons_mc_lock) { - _actuators_out_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - - } else { - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon - } - - break; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; - case FIXED_WING: - // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + if (_vtol_schedule.flight_mode == FW_MODE) { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]; // pitch elevon - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle - break; - - case TRANSITION_TO_FW: - case TRANSITION_TO_MC: - // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] - * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * - _mc_yaw_weight; + } else { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + } + + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) { + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; - // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] - * (1 - _mc_yaw_weight); + } else { + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - break; + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index b65a4ef7131d..da5715b2e0c6 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -46,30 +46,32 @@ #include /** is it necsacery? **/ #include #include +#include class Tailsitter : public VtolType { public: Tailsitter(VtolAttitudeControl *_att_controller); - ~Tailsitter() = default; + ~Tailsitter() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void update_mc_state(); - virtual void update_fw_state(); - virtual void fill_actuator_outputs(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; private: struct { float front_trans_dur_p2; - } _params_tailsitter; + float fw_pitch_sp_offset; + } _params_tailsitter{}; struct { param_t front_trans_dur_p2; - } _params_handles_tailsitter; + param_t fw_pitch_sp_offset; + } _params_handles_tailsitter{}; enum vtol_mode { MC_MODE = 0, /**< vtol is in multicopter mode */ @@ -83,14 +85,11 @@ class Tailsitter : public VtolType hrt_abstime transition_start; /**< absoulte time at which front transition started */ } _vtol_schedule; - float _thrust_transition_start; // throttle value when we start the front transition - float _yaw_transition; // yaw angle in which transition will take place - float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + matrix::Quatf _q_trans_start; + matrix::Quatf _q_trans_sp; + matrix::Vector3f _trans_rot_axis; - /** - * Update parameters. - */ - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c6424ed4e43b..04a58a46def3 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -45,8 +45,7 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : - VtolType(attc), - _tilt_control(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -61,8 +60,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN"); - _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); } void @@ -85,16 +82,10 @@ Tiltrotor::parameters_update() /* vtol front transition phase 2 duration */ param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); _params_tiltrotor.front_trans_dur_p2 = v; - - param_get(_params_handles_tiltrotor.diff_thrust, &_params_tiltrotor.diff_thrust); - - param_get(_params_handles_tiltrotor.diff_thrust_scale, &v); - _params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); } void Tiltrotor::update_vtol_state() { - /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting rotors, picking up * forward speed. After the vehicle has picked up enough speed the rotors are tilted @@ -270,7 +261,7 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = _mc_roll_weight; } - _thrust_transition = _mc_virtual_att_sp->thrust; + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely @@ -289,7 +280,7 @@ void Tiltrotor::update_transition_state() _motor_state = set_motor_state(_motor_state, VALUE, rear_value); - _thrust_transition = _mc_virtual_att_sp->thrust; + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_motor_state != ENABLED) { @@ -335,7 +326,7 @@ void Tiltrotor::update_transition_state() void Tiltrotor::waiting_on_tecs() { // keep multicopter thrust until we get data from TECS - _v_att_sp->thrust = _thrust_transition; + _v_att_sp->thrust_body[0] = _thrust_transition; } /** @@ -358,9 +349,9 @@ void Tiltrotor::fill_actuator_outputs() _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_params_tiltrotor.diff_thrust == 1) { + if (_params->diff_thrust == 1) { _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params_tiltrotor.diff_thrust_scale; + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; } } else { diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 508973642a69..edfe89dd4f75 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -50,14 +50,14 @@ class Tiltrotor : public VtolType public: Tiltrotor(VtolAttitudeControl *_att_controller); - ~Tiltrotor() = default; + ~Tiltrotor() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void fill_actuator_outputs(); - virtual void update_mc_state(); - virtual void update_fw_state(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void fill_actuator_outputs() override; + void update_mc_state() override; + void update_fw_state() override; + void waiting_on_tecs() override; private: @@ -66,8 +66,6 @@ class Tiltrotor : public VtolType float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ float tilt_fw; /**< actuator value corresponding to fw tilt */ float front_trans_dur_p2; - int32_t diff_thrust; - float diff_thrust_scale; } _params_tiltrotor; struct { @@ -75,8 +73,6 @@ class Tiltrotor : public VtolType param_t tilt_transition; param_t tilt_fw; param_t front_trans_dur_p2; - param_t diff_thrust; - param_t diff_thrust_scale; } _params_handles_tiltrotor; enum vtol_mode { @@ -99,12 +95,9 @@ class Tiltrotor : public VtolType hrt_abstime transition_start; /**< absoulte time at which front transition started */ } _vtol_schedule; - float _tilt_control; /**< actuator value for the tilt servo */ + float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ - /** - * Update parameters. - */ - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d3cb6df264e4..bf57b9687e63 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -85,28 +85,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); - -/** - * Differential thrust in forwards flight. - * - * Set to 1 to enable differential thrust in fixed-wing flight. - * - * @min 0 - * @max 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); - -/** - * Differential thrust scaling factor - * - * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a6e673b50a5a..1180875cc24c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -86,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); + _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN"); + _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); /* fetch initial parameter values */ parameters_update(); @@ -486,6 +488,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise); param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); + param_get(_params_handles.diff_thrust, &_params.diff_thrust); + + param_get(_params_handles.diff_thrust_scale, &v); + _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); // standard vtol always needs to turn all mc motors off when going into fixed wing mode // normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code @@ -506,52 +512,6 @@ VtolAttitudeControl::parameters_update() return OK; } -/** -* Prepare message for mc attitude rates setpoint topic -*/ -void VtolAttitudeControl::fill_mc_att_rates_sp() -{ - bool updated; - orb_check(_mc_virtual_v_rates_sp_sub, &updated); - - if (updated) { - vehicle_rates_setpoint_s v_rates_sp; - - if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { - // publish the attitude rates setpoint - if (_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); - } - } - } -} - -/** -* Prepare message for fw attitude rates setpoint topic -*/ -void VtolAttitudeControl::fill_fw_att_rates_sp() -{ - bool updated; - orb_check(_fw_virtual_v_rates_sp_sub, &updated); - - if (updated) { - vehicle_rates_setpoint_s v_rates_sp; - - if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { - // publish the attitude rates setpoint - if (_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); - } - } - } -} - int VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -567,8 +527,6 @@ void VtolAttitudeControl::task_main() _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); - _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); - _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -680,7 +638,6 @@ void VtolAttitudeControl::task_main() // got data from mc attitude controller _vtol_type->update_mc_state(); - fill_mc_att_rates_sp(); } else if (_vtol_type->get_mode() == FIXED_WING) { @@ -692,7 +649,6 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.in_transition_to_fw = false; _vtol_type->update_fw_state(); - fill_fw_att_rates_sp(); } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { @@ -705,7 +661,6 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW); _vtol_type->update_transition_state(); - fill_mc_att_rates_sp(); } _vtol_type->fill_actuator_outputs(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 44298866649c..40dbeec9f497 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -76,7 +76,6 @@ #include #include #include -#include #include #include "tiltrotor.h" @@ -128,13 +127,11 @@ class VtolAttitudeControl int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs int _airspeed_sub{-1}; // airspeed subscription int _fw_virtual_att_sp_sub{-1}; - int _fw_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription int _land_detected_sub{-1}; int _local_pos_sp_sub{-1}; // setpoint subscription int _local_pos_sub{-1}; // sensor subscription int _manual_control_sp_sub{-1}; //manual control setpoint subscription int _mc_virtual_att_sp_sub{-1}; - int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription int _params_sub{-1}; //parameter updates subscription int _pos_sp_triplet_sub{-1}; // local position setpoint subscription int _tecs_status_sub{-1}; @@ -148,7 +145,6 @@ class VtolAttitudeControl orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _v_att_sp_pub{nullptr}; orb_advert_t _v_cmd_ack_pub{nullptr}; - orb_advert_t _v_rates_sp_pub{nullptr}; orb_advert_t _vtol_vehicle_status_pub{nullptr}; orb_advert_t _actuators_1_pub{nullptr}; @@ -199,6 +195,8 @@ class VtolAttitudeControl param_t front_trans_timeout; param_t mpc_xy_cruise; param_t fw_motors_off; + param_t diff_thrust; + param_t diff_thrust_scale; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines @@ -229,10 +227,7 @@ class VtolAttitudeControl void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values - int parameters_update(); //Update local paraemter cache - - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); + int parameters_update(); //Update local parameter cache void handle_command(); }; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index f27da247d4d3..e8af9384fd95 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -289,3 +289,39 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); + +/** + * Differential thrust in forwards flight. + * + * Set to 1 to enable differential thrust in fixed-wing flight. + * + * @min 0 + * @max 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); + +/** + * Differential thrust scaling factor + * + * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); + +/** + * Airspeed rules regarding fixed wing control surface scaling. + * + * @value 0 No special rules + * @value 1 During hover (excluding transitions) use the lowest possible airspeed value. + * @min 0.0 + * @increment 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_AIRSPD_RULE, 0); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1a10d4c1081d..230e615bdda0 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -68,6 +68,8 @@ struct Params { float front_trans_timeout; float mpc_xy_cruise; int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + int32_t diff_thrust; + float diff_thrust_scale; }; // Has to match 1:1 msg/vtol_vehicle_status.msg diff --git a/src/systemcmds/tests/test_controlmath.cpp b/src/systemcmds/tests/test_controlmath.cpp index 7be4d0909b5a..fb5c3d6a2a5a 100644 --- a/src/systemcmds/tests/test_controlmath.cpp +++ b/src/systemcmds/tests/test_controlmath.cpp @@ -37,7 +37,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* expected: same as before but with 90 yaw * reason: only yaw changed @@ -47,7 +47,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* expected: same as before but roll 180 * reason: thrust points straight down and order Euler @@ -58,7 +58,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(fabsf(att.roll_body) - M_PI_F < SIGMA_SINGLE_OP); ut_assert_true(fabsf(att.pitch_body) < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* TODO: find a good way to test it */ From 30dbfd99fb096190531e4e4a315de62096f9967e Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Fri, 7 Sep 2018 10:42:29 +0200 Subject: [PATCH 0081/1046] EKF: Estimate WGS84 altitude with filtered AMSL offset --- msg/vehicle_global_position.msg | 1 + src/modules/ekf2/ekf2_main.cpp | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 766c3db4533c..e000549683f3 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -10,6 +10,7 @@ uint64 timestamp # time since system start (microseconds) float64 lat # Latitude, (degrees) float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) +float32 alt_ellipsoid # Altitude above ellipsoid, (meters) float32 delta_alt # Reset delta for altitude uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index fa6c93670fc1..f9b26814a3b8 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -160,6 +160,11 @@ class Ekf2 final : public ModuleBase, public ModuleParams */ void calc_gps_blend_output(); + /* + * Calculate filtered WGS84 height from estimated AMSL height + */ + float filter_altitude_ellipsoid(float amsl_hgt); + bool _replay_mode = false; ///< true when we use replay data from a log // time slip monitoring @@ -243,6 +248,10 @@ class Ekf2 final : public ModuleBase, public ModuleParams float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds. bool _gps_new_output_data = false; ///< true if there is new output data for the EKF + int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt + float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + int _airdata_sub{-1}; int _airspeed_sub{-1}; int _ev_odom_sub{-1}; @@ -979,6 +988,7 @@ void Ekf2::run() _gps_state[0].nsats = gps.satellites_used; //TODO: add gdop to gps topic _gps_state[0].gdop = 0.0f; + _gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid; ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } @@ -1010,6 +1020,7 @@ void Ekf2::run() _gps_state[1].nsats = gps.satellites_used; //TODO: add gdop to gps topic _gps_state[1].gdop = 0.0f; + _gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid; } } @@ -1399,6 +1410,7 @@ void Ekf2::run() global_pos.lat_lon_reset_counter = lpos.xy_reset_counter; global_pos.alt = -lpos.z + lpos.ref_alt; // Altitude AMSL in meters + global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); // global altitude has opposite sign of local down position global_pos.delta_alt = -lpos.delta_z; @@ -2352,6 +2364,27 @@ void Ekf2::calc_gps_blend_output() } +float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) +{ + + float height_diff = static_cast(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt; + + if (_gps_alttitude_ellipsoid_previous_timestamp[0] == 0) { + + _wgs84_hgt_offset = height_diff; + _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec; + + } else if (_gps_state[0].time_usec != _gps_alttitude_ellipsoid_previous_timestamp[0]) { + + // apply a 10 second first order low pass filter to baro offset + float dt = 1e-6f * static_cast(_gps_state[0].time_usec - _gps_alttitude_ellipsoid_previous_timestamp[0]); + _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec; + float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset); + _wgs84_hgt_offset += dt * math::constrain(offset_rate_correction, -0.1f, 0.1f); + } + + return amsl_hgt + _wgs84_hgt_offset; +} Ekf2 *Ekf2::instantiate(int argc, char *argv[]) { From 15439fcc0b2373d11c6a78359af67e8b9a9fc76d Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Fri, 7 Sep 2018 10:43:56 +0200 Subject: [PATCH 0082/1046] Mavlink: Add UTM_GLOBAL_POSITION stream --- src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/mavlink/mavlink_messages.cpp | 167 +++++++++++++++++++++++ 2 files changed, 170 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index eebf92e685d7..0687a7a215a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,6 +1773,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f); configure_stream_local("WIND_COV", 1.0f); @@ -1814,6 +1815,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIMESYNC", 10.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f); configure_stream_local("WIND_COV", 10.0f); @@ -1878,6 +1880,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIMESYNC", 10.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f); configure_stream_local("WIND_COV", 10.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5ae3b873c10d..f5ec96944b39 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1680,6 +1680,172 @@ class MavlinkStreamADSBVehicle : public MavlinkStream } }; +class MavlinkStreamUTMGlobalPosition : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamUTMGlobalPosition::get_name_static(); + } + + static const char *get_name_static() + { + return "UTM_GLOBAL_POSITION"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamUTMGlobalPosition(mavlink); + } + + bool const_rate() + { + return true; + } + + unsigned get_size() + { + return _local_pos_time > 0 ? MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time = 0; + + MavlinkOrbSubscription *_local_pos_sub; + uint64_t _local_pos_time = 0; + + MavlinkOrbSubscription *_position_setpoint_triplet_sub; + uint64_t _setpoint_triplet_time = 0; + position_setpoint_triplet_s _setpoint_triplet = {}; + + MavlinkOrbSubscription *_vehicle_status_sub; + uint64_t _vehicle_status_time = 0; + vehicle_status_s _vehicle_status = {}; + + MavlinkOrbSubscription *_land_detected_sub; + uint64_t _land_detected_time = 0; + vehicle_land_detected_s _land_detected = {}; + + /* do not allow top copying this class */ + MavlinkStreamUTMGlobalPosition(MavlinkStreamUTMGlobalPosition &) = delete; + MavlinkStreamUTMGlobalPosition &operator = (const MavlinkStreamUTMGlobalPosition &) = delete; + +protected: + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _position_setpoint_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), + _vehicle_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _land_detected_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))) + {} + + bool send(const hrt_abstime t) + { + vehicle_global_position_s global_position = {}; + vehicle_local_position_s local_position = {}; + + bool global_updated = _global_pos_sub->update(&_global_pos_time, &global_position); + bool local_updated = _local_pos_sub->update(&_local_pos_time, &local_position); + + // Check if new setpoint, vehicle status and land detected are available otherwise use the last received + _position_setpoint_triplet_sub->update(&_setpoint_triplet_time, &_setpoint_triplet); + _vehicle_status_sub->update(&_vehicle_status_time, &_vehicle_status); + _land_detected_sub->update(&_land_detected_time, &_land_detected); + + mavlink_utm_global_position_t msg = {}; + + // Compute Unix epoch and set time field + timespec tv; + px4_clock_gettime(CLOCK_REALTIME, &tv); + uint64_t unix_epoch = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + // If the time is before 2001-01-01, it's probably the default 2000 + if (unix_epoch > 978307200000000) { + msg.time = unix_epoch; + msg.flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID; + } + + // TODO Fill ID with something reasonable + memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); + //msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; + + // Handle global position update + if (global_updated) { + msg.lat = global_position.lat * 1e7; + msg.lon = global_position.lon * 1e7; + msg.alt = global_position.alt_ellipsoid * 1000.0f; + + msg.h_acc = global_position.eph * 1000.0f; + msg.v_acc = global_position.epv * 1000.0f; + + msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + } + + // Handle local position update + if (local_updated) { + + if (local_position.v_xy_valid) { + msg.vx = local_position.vx * 100.0f; + msg.vy = local_position.vy * 100.0f; + } + + msg.vel_acc = sqrtf(local_position.evh * local_position.evh + + local_position.evv * local_position.evv) * 1000.0f; + + if (local_position.v_z_valid) { + msg.vz = local_position.vz * 100.0f; + } + + if (local_position.dist_bottom_valid) { + msg.relative_alt = local_position.dist_bottom * 1000.0f; // TODO Check if terrain height is correct field + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + } + + // Handle next waypoint if it is valid + if (_setpoint_triplet_time > 0 && _setpoint_triplet.next.valid) { + msg.next_lat = _setpoint_triplet.current.lat * 1e7; + msg.next_lon = _setpoint_triplet.current.lon * 1e7; + // HACK We assume that the offset between AMSL and WGS84 is constant between the current + // vehicle position and the the target waypoint. + msg.next_alt = (_setpoint_triplet.current.alt + (global_position.alt_ellipsoid - global_position.alt)) * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; + } + + // Handle flight state + if (_vehicle_status_time > 0 && _land_detected_time > 0 + && _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (_land_detected.landed) { + msg.flight_state |= UTM_FLIGHT_STATE_GROUND; + + } else { + msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; + } + + } else { + msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; + } + + msg.update_rate = 0; // Data driven mode + + mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); + } + + return true; + } +}; + class MavlinkStreamCollision : public MavlinkStream { public: @@ -4629,6 +4795,7 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static), StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static), StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static), + StreamListItem(&MavlinkStreamUTMGlobalPosition::new_instance, &MavlinkStreamUTMGlobalPosition::get_name_static, &MavlinkStreamUTMGlobalPosition::get_id_static), StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static), StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static), From ae4654f36a05357c2d60036115bcaa043f7800d4 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Mon, 17 Sep 2018 16:09:54 +0200 Subject: [PATCH 0083/1046] Mavlink: Include new UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE flag --- src/modules/mavlink/mavlink_messages.cpp | 47 ++++++++++++------------ 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index f5ec96944b39..7e4a1c588a9c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1719,12 +1719,13 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream } private: - MavlinkOrbSubscription *_global_pos_sub; - uint64_t _global_pos_time = 0; - MavlinkOrbSubscription *_local_pos_sub; uint64_t _local_pos_time = 0; + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time = 0; + vehicle_global_position_s _global_position = {}; + MavlinkOrbSubscription *_position_setpoint_triplet_sub; uint64_t _setpoint_triplet_time = 0; position_setpoint_triplet_s _setpoint_triplet = {}; @@ -1743,8 +1744,8 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream protected: explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink), - _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _position_setpoint_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _vehicle_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _land_detected_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))) @@ -1752,13 +1753,13 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream bool send(const hrt_abstime t) { - vehicle_global_position_s global_position = {}; vehicle_local_position_s local_position = {}; - bool global_updated = _global_pos_sub->update(&_global_pos_time, &global_position); bool local_updated = _local_pos_sub->update(&_local_pos_time, &local_position); - // Check if new setpoint, vehicle status and land detected are available otherwise use the last received + // Check if new global position, setpoint, vehicle status and land detected are available + // otherwise use the last received + _global_pos_sub->update(&_global_pos_time, &_global_position); _position_setpoint_triplet_sub->update(&_setpoint_triplet_time, &_setpoint_triplet); _vehicle_status_sub->update(&_vehicle_status_time, &_vehicle_status); _land_detected_sub->update(&_land_detected_time, &_land_detected); @@ -1780,21 +1781,21 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); //msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; - // Handle global position update - if (global_updated) { - msg.lat = global_position.lat * 1e7; - msg.lon = global_position.lon * 1e7; - msg.alt = global_position.alt_ellipsoid * 1000.0f; + // Handle local position update + if (local_updated) { - msg.h_acc = global_position.eph * 1000.0f; - msg.v_acc = global_position.epv * 1000.0f; + if (_global_pos_time > 0) { + // Handle global position update + msg.lat = _global_position.lat * 1e7; + msg.lon = _global_position.lon * 1e7; + msg.alt = _global_position.alt_ellipsoid * 1000.0f; - msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; - msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; - } + msg.h_acc = _global_position.eph * 1000.0f; + msg.v_acc = _global_position.epv * 1000.0f; - // Handle local position update - if (local_updated) { + msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + } if (local_position.v_xy_valid) { msg.vx = local_position.vx * 100.0f; @@ -1809,17 +1810,17 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream } if (local_position.dist_bottom_valid) { - msg.relative_alt = local_position.dist_bottom * 1000.0f; // TODO Check if terrain height is correct field - msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + msg.relative_alt = local_position.dist_bottom * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; } // Handle next waypoint if it is valid - if (_setpoint_triplet_time > 0 && _setpoint_triplet.next.valid) { + if (_setpoint_triplet_time > 0 && _setpoint_triplet.current.valid) { msg.next_lat = _setpoint_triplet.current.lat * 1e7; msg.next_lon = _setpoint_triplet.current.lon * 1e7; // HACK We assume that the offset between AMSL and WGS84 is constant between the current // vehicle position and the the target waypoint. - msg.next_alt = (_setpoint_triplet.current.alt + (global_position.alt_ellipsoid - global_position.alt)) * 1000.0f; + msg.next_alt = (_setpoint_triplet.current.alt + (_global_position.alt_ellipsoid - _global_position.alt)) * 1000.0f; msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; } From fa4156c3d0c5caf26327115c0d3ef5c96731b34f Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Tue, 18 Sep 2018 14:56:40 +0200 Subject: [PATCH 0084/1046] Mavlink: Include UTM_DATA_AVAIL_FLAGS_*_VELO_AVAILABLE flag --- src/modules/mavlink/mavlink_messages.cpp | 113 +++++++++++++---------- 1 file changed, 64 insertions(+), 49 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7e4a1c588a9c..4d106f744126 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1721,6 +1721,7 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream private: MavlinkOrbSubscription *_local_pos_sub; uint64_t _local_pos_time = 0; + vehicle_local_position_s _local_position = {}; MavlinkOrbSubscription *_global_pos_sub; uint64_t _global_pos_time = 0; @@ -1753,12 +1754,9 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream bool send(const hrt_abstime t) { - vehicle_local_position_s local_position = {}; - bool local_updated = _local_pos_sub->update(&_local_pos_time, &local_position); - - // Check if new global position, setpoint, vehicle status and land detected are available - // otherwise use the last received + // Check if new uORB messages are available otherwise use the last received + _local_pos_sub->update(&_local_pos_time, &_local_position); _global_pos_sub->update(&_global_pos_time, &_global_position); _position_setpoint_triplet_sub->update(&_setpoint_triplet_time, &_setpoint_triplet); _vehicle_status_sub->update(&_vehicle_status_time, &_vehicle_status); @@ -1781,68 +1779,85 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); //msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; - // Handle local position update - if (local_updated) { + // Handle global position + if (_global_pos_time > 0) { + msg.lat = _global_position.lat * 1e7; + msg.lon = _global_position.lon * 1e7; + msg.alt = _global_position.alt_ellipsoid * 1000.0f; - if (_global_pos_time > 0) { - // Handle global position update - msg.lat = _global_position.lat * 1e7; - msg.lon = _global_position.lon * 1e7; - msg.alt = _global_position.alt_ellipsoid * 1000.0f; + msg.h_acc = _global_position.eph * 1000.0f; + msg.v_acc = _global_position.epv * 1000.0f; - msg.h_acc = _global_position.eph * 1000.0f; - msg.v_acc = _global_position.epv * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + } - msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; - msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; - } + // Handle local position + if (_local_pos_time > 0) { + float evh = 0.0f; + float evv = 0.0f; - if (local_position.v_xy_valid) { - msg.vx = local_position.vx * 100.0f; - msg.vy = local_position.vy * 100.0f; + if (_local_position.v_xy_valid) { + msg.vx = _local_position.vx * 100.0f; + msg.vy = _local_position.vy * 100.0f; + evh = _local_position.evh; + msg.flags |= UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE; } - msg.vel_acc = sqrtf(local_position.evh * local_position.evh - + local_position.evv * local_position.evv) * 1000.0f; - - if (local_position.v_z_valid) { - msg.vz = local_position.vz * 100.0f; + if (_local_position.v_z_valid) { + msg.vz = _local_position.vz * 100.0f; + evv = _local_position.evv; + msg.flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE; } - if (local_position.dist_bottom_valid) { - msg.relative_alt = local_position.dist_bottom * 1000.0f; - msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; - } + msg.vel_acc = sqrtf(evh * evh + evv * evv) * 1000.0f; - // Handle next waypoint if it is valid - if (_setpoint_triplet_time > 0 && _setpoint_triplet.current.valid) { - msg.next_lat = _setpoint_triplet.current.lat * 1e7; - msg.next_lon = _setpoint_triplet.current.lon * 1e7; - // HACK We assume that the offset between AMSL and WGS84 is constant between the current - // vehicle position and the the target waypoint. - msg.next_alt = (_setpoint_triplet.current.alt + (_global_position.alt_ellipsoid - _global_position.alt)) * 1000.0f; - msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; + if (_local_position.dist_bottom_valid) { + msg.relative_alt = _local_position.dist_bottom * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; } + } - // Handle flight state - if (_vehicle_status_time > 0 && _land_detected_time > 0 - && _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - if (_land_detected.landed) { - msg.flight_state |= UTM_FLIGHT_STATE_GROUND; + bool vehicle_in_auto_mode = _vehicle_status_time > 0 + && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); + + // Handle next waypoint if it is valid + if (vehicle_in_auto_mode && _setpoint_triplet_time > 0 && _setpoint_triplet.current.valid) { + msg.next_lat = _setpoint_triplet.current.lat * 1e7; + msg.next_lon = _setpoint_triplet.current.lon * 1e7; + // HACK We assume that the offset between AMSL and WGS84 is constant between the current + // vehicle position and the the target waypoint. + msg.next_alt = (_setpoint_triplet.current.alt + (_global_position.alt_ellipsoid - _global_position.alt)) * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; + } - } else { - msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; - } + // Handle flight state + if (_vehicle_status_time > 0 && _land_detected_time > 0 + && _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (_land_detected.landed) { + msg.flight_state |= UTM_FLIGHT_STATE_GROUND; } else { - msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; + msg.flight_state |= UTM_FLIGHT_STATE_AIRBORNE; } - msg.update_rate = 0; // Data driven mode - - mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); + } else { + msg.flight_state |= UTM_FLIGHT_STATE_UNKNOWN; } + msg.update_rate = 0; // Data driven mode + + mavlink_msg_utm_global_position_send_struct(_mavlink->get_channel(), &msg); + return true; } }; From 33a9eb946a8a727c54aaace7c30d48bf011b690b Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Fri, 12 Oct 2018 09:10:03 +0200 Subject: [PATCH 0085/1046] Mavlink: Reduce rate of UTM_GLOBAL_POSITION stream --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0687a7a215a7..bc19c8e59295 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,7 +1773,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f); configure_stream_local("WIND_COV", 1.0f); @@ -1815,7 +1815,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIMESYNC", 10.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f); configure_stream_local("WIND_COV", 10.0f); @@ -1880,7 +1880,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIMESYNC", 10.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 25.0f); + configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f); configure_stream_local("WIND_COV", 10.0f); From a154282fef7e4f912e434589969ffff4e1da83e9 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Wed, 24 Oct 2018 09:09:49 +0200 Subject: [PATCH 0086/1046] Mavlink: Use UUID for identifier in UTM_GLOBAL_POSITION --- src/modules/mavlink/mavlink_messages.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d106f744126..55bf66d207e7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1775,9 +1775,16 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream msg.flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID; } +#ifndef BOARD_HAS_NO_UUID + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + static_assert(sizeof(px4_guid_t) == sizeof(msg.uas_id), "GUID byte length mismatch"); + memcpy(&msg.uas_id, &px4_guid, sizeof(msg.uas_id)); + msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; +#else // TODO Fill ID with something reasonable memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); - //msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; +#endif /* BOARD_HAS_NO_UUID */ // Handle global position if (_global_pos_time > 0) { From 43034922e24691e74515b2bcd8a82d59bcf72972 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Thu, 15 Nov 2018 16:22:43 +0100 Subject: [PATCH 0087/1046] Mavlink: Incorporate changes in UTM_GLOBAL_POSITION message --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 55bf66d207e7..b4f93a7603de 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1817,7 +1817,7 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream msg.flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE; } - msg.vel_acc = sqrtf(evh * evh + evv * evv) * 1000.0f; + msg.vel_acc = sqrtf(evh * evh + evv * evv) * 100.0f; if (_local_position.dist_bottom_valid) { msg.relative_alt = _local_position.dist_bottom * 1000.0f; From f9a3235709c4f2ed5438ccab813fd0e6154529aa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 22 Nov 2018 17:43:33 -0500 Subject: [PATCH 0088/1046] remove unused parameter VT_AIRSPD_RULE (#10897) - fixes #10896 --- .../fw_att_control/FixedwingAttitudeControl.cpp | 3 --- .../fw_att_control/FixedwingAttitudeControl.hpp | 2 -- .../vtol_att_control/vtol_att_control_params.c | 11 ----------- 3 files changed, 16 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index acc3e5bb9f15..b03b8af9f0e2 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -119,7 +119,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : // initialize to invalid VTOL type _parameters.vtol_type = -1; - _parameters.vtol_airspeed_rule = 0; /* fetch initial parameter values */ parameters_update(); @@ -241,7 +240,6 @@ FixedwingAttitudeControl::parameters_update() if (_vehicle_status.is_vtol) { param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); - param_get(_parameter_handles.vtol_airspeed_rule, &_parameters.vtol_airspeed_rule); } param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); @@ -439,7 +437,6 @@ FixedwingAttitudeControl::vehicle_status_poll() _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); _parameter_handles.vtol_type = param_find("VT_TYPE"); - _parameter_handles.vtol_airspeed_rule = param_find("VT_AIRSPD_RULE"); parameters_update(); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index d9c414f78db5..084c278ec985 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -202,7 +202,6 @@ class FixedwingAttitudeControl final : public ModuleBase Date: Mon, 20 Aug 2018 20:20:14 -0400 Subject: [PATCH 0089/1046] make vdev file flags and priv consistent with nuttx --- src/lib/cdev/posix/cdev_platform.hpp | 10 +++++----- src/modules/uORB/uORBDeviceNode.cpp | 18 +++++------------- .../tests/vcdev_test/vcdevtest_example.cpp | 14 +++++++------- 3 files changed, 17 insertions(+), 25 deletions(-) diff --git a/src/lib/cdev/posix/cdev_platform.hpp b/src/lib/cdev/posix/cdev_platform.hpp index ef17b8fef67a..c93938d70c61 100644 --- a/src/lib/cdev/posix/cdev_platform.hpp +++ b/src/lib/cdev/posix/cdev_platform.hpp @@ -17,12 +17,12 @@ using px4_file_operations_t = struct file_operations; using mode_t = uint32_t; struct file_t { - int flags; - void *priv; - void *vdev; + int f_oflags{0}; + void *f_priv{nullptr}; + void *vdev{nullptr}; - file_t() : flags(0), priv(nullptr), vdev(nullptr) {} - file_t(int f, void *c) : flags(f), priv(nullptr), vdev(c) {} + file_t() = default; + file_t(int f, void *c) : f_oflags(f), vdev(c) {} }; } // namespace cdev diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 21702bcc2d54..f7042b024a2a 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -33,14 +33,6 @@ #include "uORBDeviceNode.hpp" -#ifdef __PX4_NUTTX -#define FILE_FLAGS(filp) filp->f_oflags -#define FILE_PRIV(filp) filp->f_priv -#else -#define FILE_FLAGS(filp) filp->flags -#define FILE_PRIV(filp) filp->priv -#endif - #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -58,7 +50,7 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *fil } #endif - return (SubscriberData *)(FILE_PRIV(filp)); + return (SubscriberData *)(filp->f_priv); } uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size) : @@ -82,7 +74,7 @@ uORB::DeviceNode::open(cdev::file_t *filp) int ret; /* is this a publisher? */ - if (FILE_FLAGS(filp) == PX4_F_WRONLY) { + if (filp->f_oflags == PX4_F_WRONLY) { /* become the publisher if we can */ lock(); @@ -111,7 +103,7 @@ uORB::DeviceNode::open(cdev::file_t *filp) } /* is this a new subscriber? */ - if (FILE_FLAGS(filp) == PX4_F_RDONLY) { + if (filp->f_oflags == PX4_F_RDONLY) { /* allocate subscriber data */ SubscriberData *sd = new SubscriberData{}; @@ -123,7 +115,7 @@ uORB::DeviceNode::open(cdev::file_t *filp) /* If there were any previous publications, allow the subscriber to read them */ sd->generation = _generation - (_queue_size < _generation ? _queue_size : _generation); - FILE_PRIV(filp) = (void *)sd; + filp->f_priv = (void *)sd; ret = CDev::open(filp); @@ -137,7 +129,7 @@ uORB::DeviceNode::open(cdev::file_t *filp) return ret; } - if (FILE_FLAGS(filp) == 0) { + if (filp->f_oflags == 0) { return CDev::open(filp); } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index f9ca5719442c..05755881cfd4 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -120,7 +120,7 @@ class VCDevNode : public cdev::CDev int VCDevNode::open(cdev::file_t *handlep) { // Only allow one writer - if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) { errno = EBUSY; return -1; } @@ -131,9 +131,9 @@ int VCDevNode::open(cdev::file_t *handlep) return ret; } - handlep->priv = new PrivData; + handlep->f_priv = new PrivData; - if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { + if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) { _is_open_for_write = true; } @@ -142,12 +142,12 @@ int VCDevNode::open(cdev::file_t *handlep) int VCDevNode::close(cdev::file_t *handlep) { - delete (PrivData *)handlep->priv; - handlep->priv = nullptr; + delete (PrivData *)handlep->f_priv; + handlep->f_priv = nullptr; CDev::close(handlep); // Enable a new writer of the device is re-opened for write - if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { + if ((handlep->f_oflags & PX4_F_WRONLY) && _is_open_for_write) { _is_open_for_write = false; } @@ -169,7 +169,7 @@ ssize_t VCDevNode::write(cdev::file_t *handlep, const char *buffer, size_t bufle ssize_t VCDevNode::read(cdev::file_t *handlep, char *buffer, size_t buflen) { - PrivData *p = (PrivData *)handlep->priv; + PrivData *p = (PrivData *)handlep->f_priv; ssize_t chars_read = 0; PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); From 180cd949780479c3a3a3080a8a4056c70eccc857 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Aug 2018 21:49:02 -0400 Subject: [PATCH 0090/1046] uORBDeviceNode mark all overrides --- src/modules/uORB/uORBDeviceNode.hpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 56128c8c0947..34d86226b421 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -54,16 +54,22 @@ class uORB::DeviceNode : public cdev::CDev DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size = 1); ~DeviceNode(); + // no copy, assignment, move, move assignment + DeviceNode(const DeviceNode &) = delete; + DeviceNode &operator=(const DeviceNode &) = delete; + DeviceNode(DeviceNode &&) = delete; + DeviceNode &operator=(DeviceNode &&) = delete; + /** * Method to create a subscriber instance and return the struct * pointing to the subscriber as a file pointer. */ - virtual int open(cdev::file_t *filp); + int open(cdev::file_t *filp) override; /** * Method to close a subscriber for this topic. */ - virtual int close(cdev::file_t *filp); + int close(cdev::file_t *filp) override; /** * reads data from a subscriber node to the buffer provided. @@ -76,7 +82,7 @@ class uORB::DeviceNode : public cdev::CDev * @return * ssize_t the number of bytes read. */ - virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen); + ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen) override; /** * writes the published data to the internal buffer to be read by @@ -90,12 +96,12 @@ class uORB::DeviceNode : public cdev::CDev * @return ssize_t * The number of bytes that are written */ - virtual ssize_t write(cdev::file_t *filp, const char *buffer, size_t buflen); + ssize_t write(cdev::file_t *filp, const char *buffer, size_t buflen) override; /** * IOCTL control for the subscriber. */ - virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); + int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; /** * Method to publish a data to this node. @@ -179,8 +185,10 @@ class uORB::DeviceNode : public cdev::CDev void set_priority(uint8_t priority) { _priority = priority; } protected: - virtual pollevent_t poll_state(cdev::file_t *filp); - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + + pollevent_t poll_state(cdev::file_t *filp) override; + + void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) override; private: struct UpdateIntervalData { @@ -248,8 +256,4 @@ class uORB::DeviceNode : public cdev::CDev */ bool appears_updated(SubscriberData *sd); - - // disable copy and assignment operators - DeviceNode(const DeviceNode &); - DeviceNode &operator=(const DeviceNode &); }; From 023e267e9bdde5811ae2ccf4469dac26be2e1ecf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Nov 2018 23:10:48 -0500 Subject: [PATCH 0091/1046] uORB replace ORBMap with linked list --- src/include/containers/List.hpp | 31 ++--- src/lib/cdev/CDev.hpp | 14 +-- src/modules/uORB/ORBMap.hpp | 161 -------------------------- src/modules/uORB/uORBDeviceMaster.cpp | 90 +++++--------- src/modules/uORB/uORBDeviceMaster.hpp | 20 +--- src/modules/uORB/uORBDeviceNode.cpp | 6 +- src/modules/uORB/uORBDeviceNode.hpp | 41 ++++--- 7 files changed, 81 insertions(+), 282 deletions(-) delete mode 100644 src/modules/uORB/ORBMap.hpp diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 543b64d89b01..33af53276fad 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -40,37 +40,23 @@ #pragma once template -class __EXPORT ListNode +class ListNode { public: - ListNode() : _sibling() {} - virtual ~ListNode() = default; - - // no copy, assignment, move, move assignment - ListNode(const ListNode &) = delete; - ListNode &operator=(const ListNode &) = delete; - ListNode(ListNode &&) = delete; - ListNode &operator=(ListNode &&) = delete; void setSibling(T sibling) { _sibling = sibling; } - const T getSibling() { return _sibling; } + const T getSibling() const { return _sibling; } protected: - T _sibling; + + T _sibling{nullptr}; + }; template -class __EXPORT List +class List { public: - List() : _head() {} - virtual ~List() = default; - - // no copy, assignment, move, move assignment - List(const List &) = delete; - List &operator=(const List &) = delete; - List(List &&) = delete; - List &operator=(List &&) = delete; void add(T newNode) { @@ -78,8 +64,9 @@ class __EXPORT List _head = newNode; } - const T getHead() { return _head; } + const T getHead() const { return _head; } protected: - T _head; + + T _head{nullptr}; }; diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index cecaa0f3cbb8..af036e8baadd 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -155,6 +155,13 @@ class __EXPORT CDev */ virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char *get_devname() const { return _devname; } + protected: /** * Pointer to the default cdev file operations table; useful for @@ -239,13 +246,6 @@ class __EXPORT CDev */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char *get_devname() { return _devname; } - /** * Take the driver lock. * diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp deleted file mode 100644 index cb44936fda47..000000000000 --- a/src/modules/uORB/ORBMap.hpp +++ /dev/null @@ -1,161 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include - - -namespace uORB -{ -class DeviceNode; -class ORBMap; -} - -class uORB::ORBMap -{ -public: - struct Node { - struct Node *next; - const char *node_name; - uORB::DeviceNode *node; - }; - - ORBMap() : - _top(nullptr), - _end(nullptr) - { } - ~ORBMap() - { - while (_top != nullptr) { - unlinkNext(_top); - - if (_top->next == nullptr) { - free(_top); - _top = nullptr; - _end = nullptr; - } - } - } - - /** - * Insert an element with a unique name - * @param node_name name of the node. This will not be copied, so the caller has to ensure - * the pointer is valid until the node is removed from ORBMap - * @param node - */ - void insert(const char *node_name, uORB::DeviceNode *node) - { - Node **p; - - if (_top == nullptr) { - p = &_top; - - } else { - p = &_end->next; - } - - *p = (Node *)malloc(sizeof(Node)); - - if (_end) { - _end = _end->next; - - } else { - _end = _top; - } - - _end->next = nullptr; - _end->node_name = node_name; - _end->node = node; - } - - bool find(const char *node_name) - { - Node *p = _top; - - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return true; - } - - p = p->next; - } - - return false; - } - - uORB::DeviceNode *get(const char *node_name) - { - Node *p = _top; - - while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return p->node; - } - - p = p->next; - } - - return nullptr; - } - - Node *top() const - { - return _top; - } - - bool empty() const - { - return !_top; - } - -private: - void unlinkNext(Node *a) - { - Node *b = a->next; - - if (b != nullptr) { - if (_end == b) { - _end = a; - } - - a->next = b->next; - free(b); - } - } - - Node *_top; - Node *_end; -}; - diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index afccdc8e9378..bd97e951e51f 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -43,24 +43,6 @@ #include #include -#ifdef __PX4_NUTTX -#define ITERATE_NODE_MAP() \ - for (ORBMap::Node *node_iter = _node_map.top(); node_iter; node_iter = node_iter->next) -#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \ - DeviceNode *node_obj = node_iter->node; \ - const char *node_name_str = node_iter->node_name; \ - UNUSED(node_name_str); - -#else -#include -#define ITERATE_NODE_MAP() \ - for (const auto &node_iter : _node_map) -#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \ - DeviceNode *node_obj = node_iter.second; \ - const char *node_name_str = node_iter.first.c_str(); \ - UNUSED(node_name_str); -#endif - uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -121,7 +103,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } /* construct the new node */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, priority); + uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); /* if we didn't get a device, that's bad */ if (node == nullptr) { @@ -139,7 +121,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in if (ret == -EEXIST) { /* if the node exists already, get the existing one and check if * something has been published yet. */ - uORB::DeviceNode *existing_node = getDeviceNodeLocked(devpath); + uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); if ((existing_node != nullptr) && !(existing_node->is_published())) { /* nothing has been published yet, lets claim it */ @@ -156,11 +138,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } else { // add to the node map;. -#ifdef __PX4_NUTTX - _node_map.insert(devpath, node); -#else - _node_map[std::string(devpath)] = node; -#endif + _node_list.add(node); } group_tries++; @@ -184,9 +162,8 @@ void uORB::DeviceMaster::printStatistics(bool reset) bool had_print = false; lock(); - ITERATE_NODE_MAP() { - INIT_NODE_MAP_VARS(node, node_name) + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { if (node->print_statistics(reset)) { had_print = true; } @@ -212,8 +189,8 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node } } - ITERATE_NODE_MAP() { - INIT_NODE_MAP_VARS(node, node_name) + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + ++num_topics; //check if already added @@ -255,8 +232,7 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node } last_node->node = node; - int node_name_len = strlen(node_name); - last_node->instance = (uint8_t)(node_name[node_name_len - 1] - '0'); + size_t name_length = strlen(last_node->node->get_meta()->o_name); if (name_length > max_topic_name_length) { @@ -286,7 +262,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) lock(); - if (_node_map.empty()) { + if (_node_list.getHead() == nullptr) { unlock(); PX4_INFO("no active topics"); return; @@ -374,7 +350,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) if (!print_active_only || cur_node->pub_msg_delta > 0) { PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, - cur_node->node->get_meta()->o_name, (int)cur_node->instance, + cur_node->node->get_meta()->o_name, (int)cur_node->node->get_instance(), (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta, (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size()); } @@ -403,39 +379,37 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) { lock(); - uORB::DeviceNode *node = getDeviceNodeLocked(nodepath); - unlock(); - //We can safely return the node that can be used by any thread, because - //a DeviceNode never gets deleted. - return node; -} - - -#ifdef __PX4_NUTTX -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath) -{ - uORB::DeviceNode *rc = nullptr; - if (_node_map.find(nodepath)) { - rc = _node_map.get(nodepath); + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + if (strcmp(node->get_devname(), nodepath) == 0) { + unlock(); + return node; + } } - return rc; -} + unlock(); -#else + return nullptr; +} -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath) +uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) { - uORB::DeviceNode *rc = nullptr; - std::string np(nodepath); + lock(); + uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); + unlock(); - auto iter = _node_map.find(np); + //We can safely return the node that can be used by any thread, because + //a DeviceNode never gets deleted. + return node; +} - if (iter != _node_map.end()) { - rc = iter->second; +uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance) +{ + for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) { + if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) { + return node; + } } - return rc; + return nullptr; } -#endif diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 202402a988ab..242201f27553 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -45,17 +45,10 @@ class DeviceMaster; class Manager; } -#ifdef __PX4_NUTTX #include #include -#include "ORBMap.hpp" -#else - -#include -#include - -#endif /* __PX4_NUTTX */ +#include /** * Master control device for ObjDev. @@ -74,6 +67,7 @@ class uORB::DeviceMaster * @return node if exists, nullptr otherwise */ uORB::DeviceNode *getDeviceNode(const char *node_name); + uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance); /** * Print statistics for each existing topic. @@ -97,7 +91,6 @@ class uORB::DeviceMaster struct DeviceNodeStatisticsData { DeviceNode *node; - uint8_t instance; uint32_t last_lost_msg_count; unsigned int last_pub_msg_count; uint32_t lost_msg_delta; @@ -115,13 +108,10 @@ class uORB::DeviceMaster * _lock must already be held when calling this. * @return node if exists, nullptr otherwise */ - uORB::DeviceNode *getDeviceNodeLocked(const char *node_name); + uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance); + + List _node_list; -#ifdef __PX4_NUTTX - ORBMap _node_map; -#else - std::map _node_map; -#endif hrt_abstime _last_statistics_output; px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index f7042b024a2a..e176598722ec 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -53,10 +53,12 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *fil return (SubscriberData *)(filp->f_priv); } -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size) : +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, + uint8_t priority, uint8_t queue_size) : CDev(path), _meta(meta), - _priority((uint8_t)priority), + _instance(instance), + _priority(priority), _queue_size(queue_size) { } diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 34d86226b421..9e5c9916a183 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -38,6 +38,8 @@ #include +#include + namespace uORB { class DeviceNode; @@ -48,10 +50,11 @@ class Manager; /** * Per-object device instance. */ -class uORB::DeviceNode : public cdev::CDev +class uORB::DeviceNode : public cdev::CDev, public ListNode { public: - DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size = 1); + DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority, + uint8_t queue_size = 1); ~DeviceNode(); // no copy, assignment, move, move assignment @@ -175,11 +178,19 @@ class uORB::DeviceNode : public cdev::CDev */ bool print_statistics(bool reset); - unsigned int get_queue_size() const { return _queue_size; } + uint8_t get_queue_size() const { return _queue_size; } + int8_t subscriber_count() const { return _subscriber_count; } + uint32_t lost_message_count() const { return _lost_messages; } - unsigned int published_message_count() const { return _generation; } - const struct orb_metadata *get_meta() const { return _meta; } + + unsigned published_message_count() const { return _generation; } + + const orb_metadata *get_meta() const { return _meta; } + + const char *get_name() const { return _meta->o_name; } + + uint8_t get_instance() const { return _instance; } int get_priority() const { return _priority; } void set_priority(uint8_t priority) { _priority = priority; } @@ -211,7 +222,8 @@ class uORB::DeviceNode : public cdev::CDev { if (update_interval) { update_interval->update_reported = update_reported_flag; } } }; - const struct orb_metadata *_meta; /**< object metadata information */ + const orb_metadata *_meta; /**< object metadata information */ + const uint8_t _instance; /**< orb multi instance identifier */ uint8_t *_data{nullptr}; /**< allocated object buffer */ hrt_abstime _last_update{0}; /**< time the object was last updated */ volatile unsigned _generation{0}; /**< object generation count */ @@ -220,19 +232,14 @@ class uORB::DeviceNode : public cdev::CDev uint8_t _queue_size; /**< maximum number of elements in the queue */ int8_t _subscriber_count{0}; - inline static SubscriberData *filp_to_sd(cdev::file_t *filp); + px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call. + We allow one publisher to have an open file descriptor at the same time. */ -#ifdef __PX4_NUTTX - pid_t _publisher {0}; /**< if nonzero, current publisher. Only used inside the advertise call. - We allow one publisher to have an open file descriptor at the same time. */ -#else - px4_task_t _publisher {0}; /**< if nonzero, current publisher. Only used inside the advertise call. - We allow one publisher to have an open file descriptor at the same time. */ -#endif + // statistics + uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same + message, it is counted as two. */ - //statistics - uint32_t _lost_messages = 0; ///< nr of lost messages for all subscribers. If two subscribers lose the same - ///message, it is counted as two. + inline static SubscriberData *filp_to_sd(cdev::file_t *filp); /** * Perform a deferred update for a rate-limited subscriber. From a6684c6f9a639b26c10b7a72e79043281ea1fd3d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Nov 2018 23:13:13 -0500 Subject: [PATCH 0092/1046] uORB orb_exists use internal linked list instead of file path --- src/modules/uORB/uORBManager.cpp | 34 +++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 027626c45bd9..20c9bf843298 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -94,34 +94,44 @@ uORB::DeviceMaster *uORB::Manager::get_device_master() int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { + int ret = PX4_ERROR; + + // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { + return ret; + } + + if (get_device_master()) { + uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance); + + if (node != nullptr) { + if (node->is_published()) { + return PX4_OK; + } + } + } + +#ifdef ORB_COMMUNICATOR + /* * Generate the path to the node and try to open it. */ char path[orb_maxpath]; int inst = instance; - int ret = uORB::Utils::node_mkpath(path, meta, &inst); + + ret = uORB::Utils::node_mkpath(path, meta, &inst); if (ret != OK) { errno = -ret; return PX4_ERROR; } -#if defined(__PX4_NUTTX) - struct stat buffer; - ret = stat(path, &buffer); -#else ret = px4_access(path, F_OK); -#ifdef ORB_COMMUNICATOR - if (ret == -1 && meta != nullptr && !_remote_topics.empty()) { ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR; } -#endif /* ORB_COMMUNICATOR */ - -#endif - if (ret == 0) { // we know the topic exists, but it's not necessarily advertised/published yet (for example // if there is only a subscriber) @@ -141,6 +151,8 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) } } +#endif /* ORB_COMMUNICATOR */ + return ret; } From 20f870137b66c61df0ba0dcd6a96a8deab01f92e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 23 Nov 2018 09:18:14 +0100 Subject: [PATCH 0093/1046] FW auto - Add parameter for flaps setting during landing --- .../fw_att_control/FixedwingAttitudeControl.cpp | 5 ++++- .../fw_att_control/FixedwingAttitudeControl.hpp | 2 ++ src/modules/fw_att_control/fw_att_control_params.c | 14 ++++++++++++++ 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b03b8af9f0e2..fc94ad4babdd 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -110,6 +110,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); _parameter_handles.flaps_takeoff_scale = param_find("FW_FLAPS_TO_SCL"); + _parameter_handles.flaps_land_scale = param_find("FW_FLAPS_LND_SCL"); _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); _parameter_handles.rattitude_thres = param_find("FW_RATT_TH"); @@ -234,6 +235,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); param_get(_parameter_handles.flaps_takeoff_scale, &_parameters.flaps_takeoff_scale); + param_get(_parameter_handles.flaps_land_scale, &_parameters.flaps_land_scale); param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres); @@ -929,7 +931,8 @@ void FixedwingAttitudeControl::control_flaps(const float dt) case vehicle_attitude_setpoint_s::FLAPS_OFF : flap_control = 0.0f; // no flaps break; - case vehicle_attitude_setpoint_s::FLAPS_LAND : flap_control = 1.0f * _parameters.flaps_scale; // landing flaps + case vehicle_attitude_setpoint_s::FLAPS_LAND : flap_control = 1.0f * _parameters.flaps_scale * + _parameters.flaps_land_scale; // landing flaps break; case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF : flap_control = 1.0f * _parameters.flaps_scale * diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 084c278ec985..0867b6390036 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -197,6 +197,7 @@ class FixedwingAttitudeControl final : public ModuleBase Date: Sat, 17 Nov 2018 13:51:20 +0100 Subject: [PATCH 0094/1046] Always use FILE* for standard output. The threads running commands for clients through the Posix daemon used to write to a char buffer through snprintf (etc.) which was then written directly to the file descriptor, whereas in the other case printf (etc.) was used to write to stdout (FILE*). Both versions used some macro's and repeated code to have the same output. This change unifies these two cases by using a FILE* in both cases. The (line) buffering is done by the standard C library's implementation (just like with stdout), and px4_log.c now uses the same code in all cases (using fprintf, etc.) for printing (colored) output. --- .../posix/include/px4_daemon/server_io.h | 25 +--- platforms/posix/src/px4_daemon/client.cpp | 1 + platforms/posix/src/px4_daemon/server.cpp | 54 +++++-- platforms/posix/src/px4_daemon/server.h | 4 +- platforms/posix/src/px4_daemon/server_io.cpp | 51 +++---- src/drivers/linux_pwm_out/navio_sysfs.cpp | 1 + src/drivers/linux_pwm_out/ocpoc_mmap.cpp | 1 + .../navio_sysfs_rc_in/navio_sysfs_rc_in.cpp | 1 + src/drivers/rpi_rc_in/rpi_rc_in.h | 1 + src/modules/logger/util.cpp | 1 + .../uORB/uORB_tests/uORBTest_UnitTest.hpp | 1 + src/platforms/common/CMakeLists.txt | 2 +- src/platforms/common/px4_log.c | 134 +++++------------- src/platforms/px4_log.h | 18 --- src/systemcmds/tests/test_hysteresis.cpp | 1 + src/systemcmds/tests/test_microbench_hrt.cpp | 1 + src/systemcmds/tests/test_microbench_math.cpp | 1 + .../tests/test_microbench_matrix.cpp | 1 + src/systemcmds/tests/test_microbench_uorb.cpp | 1 + src/systemcmds/tests/test_mixer.cpp | 1 + src/systemcmds/tests/test_parameters.cpp | 1 + 21 files changed, 113 insertions(+), 189 deletions(-) diff --git a/platforms/posix/include/px4_daemon/server_io.h b/platforms/posix/include/px4_daemon/server_io.h index 88c417501dea..efeae545b861 100644 --- a/platforms/posix/include/px4_daemon/server_io.h +++ b/platforms/posix/include/px4_daemon/server_io.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,11 +33,7 @@ /** * @file server_io.h * - * These are helper functions to send the stdout over a pipe - * back to the client. - * - * @author Julian Oes - * @author Beat Küng + * @author Mara Bos */ #pragma once @@ -46,20 +42,11 @@ __BEGIN_DECLS /** - * Get the stdout pipe buffer in order to write to fill it. + * Get the stdout of the current thread. * - * @param buffer: pointer to buffer that will be set in function. - * @param max_length: length of the assigned buffer. - * @param is_atty: true if we are writing to a terminal (and can e.g. use colors). - * @return 0 on success + * @param isatty_: if not NULL, *isatty_ will be set to wether the stream points to a terminal (true) or not (false). + * @return The FILE* which represents the standard output of the current thread. */ -__EXPORT int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty); +__EXPORT FILE *get_stdout(bool *isatty_); -/** - * Write the filled bytes to the pipe. - * - * @param buffer_length: the number of bytes that should be written. - * @return 0 on success - */ -__EXPORT int send_stdout_pipe_buffer(unsigned buffer_length); __END_DECLS diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4_daemon/client.cpp index f6604b9566cb..6266489a5e88 100644 --- a/platforms/posix/src/px4_daemon/client.cpp +++ b/platforms/posix/src/px4_daemon/client.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4_daemon/server.cpp index d327385507cd..6df0507ccd85 100644 --- a/platforms/posix/src/px4_daemon/server.cpp +++ b/platforms/posix/src/px4_daemon/server.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -141,6 +142,10 @@ Server::_server_main() // Watch the listening socket for incoming connections. poll_fds.push_back(pollfd {_fd, POLLIN, 0}); + // The list of FILE pointers that we'll need to fclose(). + // stdouts[i] corresponds to poll_fds[i+1]. + std::vector stdouts; + while (true) { int n_ready = poll(poll_fds.data(), poll_fds.size(), -1); @@ -162,22 +167,36 @@ Server::_server_main() return; } - // Start a new thread to handle the client. - pthread_t *thread = &_fd_to_thread[client]; - ret = pthread_create(thread, nullptr, Server::_handle_client, (void *)(intptr_t)client); + FILE *thread_stdout = fdopen(client, "w"); - if (ret != 0) { - PX4_ERR("could not start pthread (%i)", ret); - _fd_to_thread.erase(client); + if (thread_stdout == nullptr) { + PX4_ERR("could not open stdout for new thread"); close(client); } else { - // We won't join the thread, so detach to automatically release resources at its end - pthread_detach(*thread); - } + // Set stream to line buffered. + setvbuf(thread_stdout, nullptr, _IOLBF, BUFSIZ); + + // Start a new thread to handle the client. + pthread_t *thread = &_fd_to_thread[client]; + ret = pthread_create(thread, nullptr, Server::_handle_client, thread_stdout); + + if (ret != 0) { + PX4_ERR("could not start pthread (%i)", ret); + _fd_to_thread.erase(client); + fclose(thread_stdout); + + } else { + // We won't join the thread, so detach to automatically release resources at its end + pthread_detach(*thread); - // Start listening for the client hanging up. - poll_fds.push_back(pollfd {client, POLLHUP, 0}); + // Start listening for the client hanging up. + poll_fds.push_back(pollfd {client, POLLHUP, 0}); + + // Remember the FILE *, so we can fclose() it later. + stdouts.push_back(thread_stdout); + } + } } // Handle any closed connections. @@ -193,7 +212,8 @@ Server::_server_main() _fd_to_thread.erase(thread); } - close(poll_fds[i].fd); + fclose(stdouts[i - 1]); + stdouts.erase(stdouts.begin() + i - 1); poll_fds.erase(poll_fds.begin() + i); } else { @@ -210,7 +230,8 @@ Server::_server_main() void *Server::_handle_client(void *arg) { - int fd = (int)(intptr_t)arg; + FILE *out = (FILE *)arg; + int fd = fileno(out); // Read until the end of the incoming stream. std::string cmd; @@ -247,7 +268,7 @@ void if ((thread_data_ptr = (CmdThreadSpecificData *)pthread_getspecific(_instance->_key)) == nullptr) { thread_data_ptr = new CmdThreadSpecificData; - thread_data_ptr->fd = fd; + thread_data_ptr->thread_stdout = out; thread_data_ptr->is_atty = isatty; (void)pthread_setspecific(_instance->_key, (void *)thread_data_ptr); @@ -259,10 +280,13 @@ void // Report return value. char buf[2] = {0, (char)retval}; - if (write(fd, buf, sizeof buf) < 0) { + if (fwrite(buf, sizeof buf, 1, out) != 1) { // Don't care it went wrong, as we're cleaning up anyway. } + // Flush the FILE*'s buffer before we shut down the connection. + fflush(out); + _cleanup(fd); return nullptr; } diff --git a/platforms/posix/src/px4_daemon/server.h b/platforms/posix/src/px4_daemon/server.h index 1ce8d0767595..2932a977ff29 100644 --- a/platforms/posix/src/px4_daemon/server.h +++ b/platforms/posix/src/px4_daemon/server.h @@ -49,6 +49,7 @@ */ #pragma once +#include #include #include #include @@ -75,9 +76,8 @@ class Server int start(); struct CmdThreadSpecificData { - int fd; // fd to send stdout to + FILE *thread_stdout; // stdout of this thread bool is_atty; // whether file descriptor refers to a terminal - char buffer[1024]; }; static bool is_running() diff --git a/platforms/posix/src/px4_daemon/server_io.cpp b/platforms/posix/src/px4_daemon/server_io.cpp index ba801053da44..381bea9be15e 100644 --- a/platforms/posix/src/px4_daemon/server_io.cpp +++ b/platforms/posix/src/px4_daemon/server_io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016-2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,8 +35,10 @@ * * @author Julian Oes * @author Beat Küng + * @author Mara Bos */ +#include #include #include #include @@ -56,21 +58,19 @@ using namespace px4_daemon; -int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty) +FILE *get_stdout(bool *isatty_) { - // The thread specific data won't be initialized if the server is not running. Server::CmdThreadSpecificData *thread_data_ptr; - if (!Server::is_running()) { - return -1; - } - // If we are not in a thread that has been started by a client, we don't // have any thread specific data set and we won't have a pipe to write // stdout to. - if ((thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific( + if (!Server::is_running() || + (thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific( Server::get_pthread_key())) == nullptr) { - return -1; + if (isatty_) { *isatty_ = isatty(1); } + + return stdout; } #ifdef __PX4_POSIX_EAGLE @@ -79,37 +79,20 @@ int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty) // even though the pthread_key has been created. // We can catch this using the check below but we have no clue why this happens. if (thread_data_ptr == (void *)0x1) { - return -1; + if (isatty_) { *isatty_ = isatty(1); } + + return stdout; } #endif - *buffer = thread_data_ptr->buffer; - *max_length = sizeof(thread_data_ptr->buffer); - *is_atty = thread_data_ptr->is_atty; - - return 0; -} - -int send_stdout_pipe_buffer(unsigned buffer_length) -{ - Server::CmdThreadSpecificData *thread_data_ptr; + if (thread_data_ptr->thread_stdout == nullptr) { + if (isatty_) { *isatty_ = isatty(1); } - if (!Server::is_running()) { - return -1; + return stdout; } - if ((thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific( - Server::get_pthread_key())) == nullptr) { - return -1; - } - - int bytes_sent = write(thread_data_ptr->fd, thread_data_ptr->buffer, buffer_length); - - if (bytes_sent != (int)buffer_length) { - printf("write fail\n"); - return -1; - } + if (isatty_) { *isatty_ = thread_data_ptr->is_atty; } - return 0; + return thread_data_ptr->thread_stdout; } diff --git a/src/drivers/linux_pwm_out/navio_sysfs.cpp b/src/drivers/linux_pwm_out/navio_sysfs.cpp index 4b5c992cc87a..8b00f499531c 100644 --- a/src/drivers/linux_pwm_out/navio_sysfs.cpp +++ b/src/drivers/linux_pwm_out/navio_sysfs.cpp @@ -35,6 +35,7 @@ #include #include +#include #include using namespace linux_pwm_out; diff --git a/src/drivers/linux_pwm_out/ocpoc_mmap.cpp b/src/drivers/linux_pwm_out/ocpoc_mmap.cpp index c301f77be19e..991aa5530070 100644 --- a/src/drivers/linux_pwm_out/ocpoc_mmap.cpp +++ b/src/drivers/linux_pwm_out/ocpoc_mmap.cpp @@ -37,6 +37,7 @@ #include #include +#include using namespace linux_pwm_out; diff --git a/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp index d53003b4f9c1..18081a09ff44 100644 --- a/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp +++ b/src/drivers/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.h b/src/drivers/rpi_rc_in/rpi_rc_in.h index def9f74a4780..b301d8fab26e 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.h +++ b/src/drivers/rpi_rc_in/rpi_rc_in.h @@ -46,6 +46,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/logger/util.cpp b/src/modules/logger/util.cpp index 9d6513927c49..958369c062d6 100644 --- a/src/modules/logger/util.cpp +++ b/src/modules/logger/util.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index a029ec7cd3bf..bb54d92bbf16 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -37,6 +37,7 @@ #include "../uORB.h" #include #include +#include struct orb_test { int val; diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 60963668a93c..fcd2eb5b04c3 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -33,7 +33,7 @@ set(SRCS) -if (NOT "${OS}" MATCHES "qurt") +if (NOT "${OS}" MATCHES "qurt" AND NOT "${CONFIG}" MATCHES "px4io") list(APPEND SRCS px4_log.c ) diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c index 5476162f9160..850710121ab0 100644 --- a/src/platforms/common/px4_log.c +++ b/src/platforms/common/px4_log.c @@ -31,6 +31,7 @@ * ****************************************************************************/ +#include #include #include #include @@ -95,78 +96,38 @@ void px4_backtrace() __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) { + FILE *out = stdout; + bool use_color = true; #ifdef __PX4_POSIX - char *buffer; - unsigned max_length; - bool is_atty = false; - - if (get_stdout_pipe_buffer(&buffer, &max_length, &is_atty) == 0) { - if (level >= _PX4_LOG_LEVEL_INFO) { - - unsigned pos = 0; - - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } - - if (pos >= max_length) { return; } - - pos += snprintf(buffer + pos, max_length - pos, __px4__log_level_fmt __px4__log_level_arg(level)); - - if (pos >= max_length) { return; } - - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_GRAY); } - - if (pos >= max_length) { return; } - - pos += snprintf(buffer + pos, max_length - pos, __px4__log_modulename_pfmt, moduleName); - va_list argptr; - - if (pos >= max_length) { return; } - - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } - - if (pos >= max_length) { return; } - - va_start(argptr, fmt); - pos += vsnprintf(buffer + pos, max_length - pos, fmt, argptr); + out = get_stdout(&use_color); +#endif - if (pos >= max_length) { return; } +#ifndef PX4_LOG_COLORIZED_OUTPUT + use_color = false; +#endif - va_end(argptr); - pos += snprintf(buffer + pos, max_length - pos, "\n"); + if (level >= _PX4_LOG_LEVEL_INFO) { + if (use_color) { fputs(__px4_log_level_color[level], out); } - if (pos >= max_length) { return; } + fprintf(out, __px4__log_level_fmt __px4__log_level_arg(level)); - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_RESET); } + if (use_color) { fputs(PX4_ANSI_COLOR_GRAY, out); } - if (pos >= max_length) { return; } + fprintf(out, __px4__log_modulename_pfmt, moduleName); - // +1 for the terminating 0 char. - send_stdout_pipe_buffer(pos + 1); - } + if (use_color) { fputs(__px4_log_level_color[level], out); } - } else { -#endif + va_list argptr; + va_start(argptr, fmt); + vfprintf(out, fmt, argptr); + va_end(argptr); - if (level >= _PX4_LOG_LEVEL_INFO) { - PX4_LOG_COLOR_START - printf(__px4__log_level_fmt __px4__log_level_arg(level)); - PX4_LOG_COLOR_MODULE - printf(__px4__log_modulename_pfmt, moduleName); - PX4_LOG_COLOR_MESSAGE - va_list argptr; - va_start(argptr, fmt); - vprintf(fmt, argptr); - va_end(argptr); - PX4_LOG_COLOR_END - printf("\n"); - } + if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); } -#ifdef __PX4_POSIX + fputc('\n', out); } -#endif - /* publish an orb log message */ if (level >= _PX4_LOG_LEVEL_WARN && orb_log_message_pub) { //only publish important messages @@ -201,52 +162,25 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char * __EXPORT void px4_log_raw(int level, const char *fmt, ...) { + FILE *out = stdout; + bool use_color = true; #ifdef __PX4_POSIX - char *buffer; - unsigned max_length; - bool is_atty = false; - - if (get_stdout_pipe_buffer(&buffer, &max_length, &is_atty) == 0) { - if (level >= _PX4_LOG_LEVEL_INFO) { - - unsigned pos = 0; - - va_list argptr; - - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } - - if (pos >= max_length) { return; } - - va_start(argptr, fmt); - pos += vsnprintf(buffer + pos, max_length - pos, fmt, argptr); - va_end(argptr); - - if (pos >= max_length) { return; } - - if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_RESET); } - - if (pos >= max_length) { return; } - - // +1 for the terminating 0 char. - send_stdout_pipe_buffer(pos + 1); - } + out = get_stdout(&use_color); +#endif - } else { +#ifndef PX4_LOG_COLORIZED_OUTPUT + use_color = false; #endif - if (level >= _PX4_LOG_LEVEL_INFO) { - PX4_LOG_COLOR_START - PX4_LOG_COLOR_MESSAGE - va_list argptr; - va_start(argptr, fmt); - vprintf(fmt, argptr); - va_end(argptr); - PX4_LOG_COLOR_END - } + if (level >= _PX4_LOG_LEVEL_INFO) { + if (use_color) { fputs(__px4_log_level_color[level], out); } -#ifdef __PX4_POSIX - } + va_list argptr; + va_start(argptr, fmt); + vfprintf(out, fmt, argptr); + va_end(argptr); -#endif + if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); } + } } diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 1e6e2208b29f..f3e72b078234 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -196,24 +196,6 @@ __END_DECLS #endif /* __PX4_POSIX */ -#ifdef PX4_LOG_COLORIZED_OUTPUT -#include -#define PX4_LOG_COLOR_START \ - int use_color = isatty(STDOUT_FILENO); \ - if (use_color) printf("%s", __px4_log_level_color[level]); -#define PX4_LOG_COLOR_MODULE \ - if (use_color) printf(PX4_ANSI_COLOR_GRAY); -#define PX4_LOG_COLOR_MESSAGE \ - if (use_color) printf("%s", __px4_log_level_color[level]); -#define PX4_LOG_COLOR_END \ - if (use_color) printf(PX4_ANSI_COLOR_RESET); -#else -#define PX4_LOG_COLOR_START -#define PX4_LOG_COLOR_MODULE -#define PX4_LOG_COLOR_MESSAGE -#define PX4_LOG_COLOR_END -#endif /* PX4_LOG_COLORIZED_OUTPUT */ - /**************************************************************************** * Output format macros * Use these to implement the code level macros below diff --git a/src/systemcmds/tests/test_hysteresis.cpp b/src/systemcmds/tests/test_hysteresis.cpp index 8569a17b4504..17d884694c41 100644 --- a/src/systemcmds/tests/test_hysteresis.cpp +++ b/src/systemcmds/tests/test_hysteresis.cpp @@ -1,4 +1,5 @@ #include +#include #include diff --git a/src/systemcmds/tests/test_microbench_hrt.cpp b/src/systemcmds/tests/test_microbench_hrt.cpp index c4a75ce5d6db..80d917280c63 100644 --- a/src/systemcmds/tests/test_microbench_hrt.cpp +++ b/src/systemcmds/tests/test_microbench_hrt.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include diff --git a/src/systemcmds/tests/test_microbench_math.cpp b/src/systemcmds/tests/test_microbench_math.cpp index 7e492a342be7..a3d50f5adf2c 100644 --- a/src/systemcmds/tests/test_microbench_math.cpp +++ b/src/systemcmds/tests/test_microbench_math.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include diff --git a/src/systemcmds/tests/test_microbench_matrix.cpp b/src/systemcmds/tests/test_microbench_matrix.cpp index aac4f97858ba..2cd7107c77a4 100644 --- a/src/systemcmds/tests/test_microbench_matrix.cpp +++ b/src/systemcmds/tests/test_microbench_matrix.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include diff --git a/src/systemcmds/tests/test_microbench_uorb.cpp b/src/systemcmds/tests/test_microbench_uorb.cpp index 85ead44fac98..9d9faff226fa 100644 --- a/src/systemcmds/tests/test_microbench_uorb.cpp +++ b/src/systemcmds/tests/test_microbench_uorb.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 0e1b270975f4..c389eb18b08d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include diff --git a/src/systemcmds/tests/test_parameters.cpp b/src/systemcmds/tests/test_parameters.cpp index 2d2e9cb7e9df..317309fc8d67 100644 --- a/src/systemcmds/tests/test_parameters.cpp +++ b/src/systemcmds/tests/test_parameters.cpp @@ -2,6 +2,7 @@ #include #include +#include class ParameterTest : public UnitTest { From d13836eb1e1919f483ab72e33251611e28170238 Mon Sep 17 00:00:00 2001 From: Mara Bos Date: Sun, 18 Nov 2018 19:05:08 +0100 Subject: [PATCH 0095/1046] Add dynamic modules on Posix. You can now add `DYNAMIC` as an option to `px4_add_module`, which will cause that module to no longer be compiled into the px4 executable, but instead produce a separate shared library file, which can be loaded and executed with the new `dyn` command: pxh> dyn ./hello.px4mod start This will load the shared object file `hello.px4mod` if it wasn't already loaded, and execute its main function with the given arguments. --- cmake/common/px4_base.cmake | 25 +++++-- cmake/configs/posix_rpi_common.cmake | 1 + cmake/configs/posix_sitl_default.cmake | 1 + platforms/posix/cmake/px4_impl_os.cmake | 5 ++ src/systemcmds/dyn/CMakeLists.txt | 8 +++ src/systemcmds/dyn/dyn.cpp | 91 +++++++++++++++++++++++++ 6 files changed, 127 insertions(+), 4 deletions(-) create mode 100644 src/systemcmds/dyn/CMakeLists.txt create mode 100644 src/systemcmds/dyn/dyn.cpp diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 60703be25ab4..58ed8fe3c686 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -163,6 +163,7 @@ endfunction() # [ SRCS ] # [ MODULE_CONFIG ] # [ EXTERNAL ] +# [ DYNAMIC ] # ) # # Input: @@ -178,10 +179,12 @@ endfunction() # INCLUDES : include directories # DEPENDS : targets which this module depends on # EXTERNAL : flag to indicate that this module is out-of-tree +# DYNAMIC : don't compile into the px4 binary, but build a separate dynamically loadable module (posix) # UNITY_BUILD : merge all source files and build this module as a single compilation unit # # Output: # Static library with name matching MODULE. +# (Or a shared library when DYNAMIC is specified.) # # Example: # px4_add_module(MODULE test @@ -198,7 +201,7 @@ function(px4_add_module) NAME px4_add_module ONE_VALUE MODULE MAIN STACK STACK_MAIN STACK_MAX PRIORITY MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS MODULE_CONFIG - OPTIONS EXTERNAL UNITY_BUILD + OPTIONS EXTERNAL DYNAMIC UNITY_BUILD REQUIRED MODULE MAIN ARGN ${ARGN}) @@ -226,16 +229,30 @@ function(px4_add_module) add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp) target_include_directories(${MODULE} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + elseif(DYNAMIC AND MAIN AND (${OS} STREQUAL "posix")) + add_library(${MODULE} SHARED ${SRCS}) + target_compile_definitions(${MODULE} PRIVATE ${MAIN}_main=px4_module_main) + set_target_properties(${MODULE} PROPERTIES + PREFIX "" + SUFFIX ".px4mod" + ) + target_link_libraries(${MODULE} PRIVATE px4) + if(APPLE) + # Postpone resolving symbols until loading time, which is the default on most systems, but not Mac. + set_target_properties(${MODULE} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") + endif() else() add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${SRCS}) endif() # all modules can potentially use parameters and uORB add_dependencies(${MODULE} uorb_headers) - target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface platforms__common px4_layer systemlib) - set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) - set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + if(NOT DYNAMIC) + target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface platforms__common px4_layer systemlib) + set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) + set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + endif() px4_add_optimization_flags_for_target(${MODULE}) diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index 0bba4a1cfeaf..2b8968b793e8 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -34,6 +34,7 @@ set(config_module_list # System commands # systemcmds/param + systemcmds/dyn systemcmds/led_control systemcmds/mixer systemcmds/ver diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index ca43114198f1..a8a5cdbb96d3 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -24,6 +24,7 @@ set(config_module_list #systemcmds/bl_update #systemcmds/config #systemcmds/dumpfile + systemcmds/dyn systemcmds/esc_calib systemcmds/led_control systemcmds/mixer diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake index ed29e8347acc..d21bccd3fc66 100644 --- a/platforms/posix/cmake/px4_impl_os.cmake +++ b/platforms/posix/cmake/px4_impl_os.cmake @@ -48,6 +48,11 @@ include(common/px4_base) list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake/posix) +# This makes it possible to dynamically load code which depends on symbols +# inside the px4 executable. +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(CMAKE_ENABLE_EXPORTS ON) + #============================================================================= # # px4_posix_generate_builtin_commands diff --git a/src/systemcmds/dyn/CMakeLists.txt b/src/systemcmds/dyn/CMakeLists.txt new file mode 100644 index 000000000000..1d2ec09f5ffb --- /dev/null +++ b/src/systemcmds/dyn/CMakeLists.txt @@ -0,0 +1,8 @@ +px4_add_module( + MODULE systemcmds__dyn + MAIN dyn + SRCS + dyn.cpp + ) + +target_link_libraries(systemcmds__dyn PUBLIC dl) diff --git a/src/systemcmds/dyn/dyn.cpp b/src/systemcmds/dyn/dyn.cpp new file mode 100644 index 000000000000..150d42f1466e --- /dev/null +++ b/src/systemcmds/dyn/dyn.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file dyn.cpp + * + * @author Mara Bos + */ + +#include + +#include +#include + +static void usage(); + +extern "C" { + __EXPORT int dyn_main(int argc, char *argv[]); +} + +static void usage() +{ + PRINT_MODULE_DESCRIPTION( + R"( +### Description +Load and run a dynamic PX4 module, which was not compiled into the PX4 binary. + +### Example +$ dyn ./hello.px4mod start + +)"); + PRINT_MODULE_USAGE_NAME_SIMPLE("dyn", "command"); + PRINT_MODULE_USAGE_ARG("", "File containing the module", false); + PRINT_MODULE_USAGE_ARG("arguments...", "Arguments to the module", true); +} + +int dyn_main(int argc, char *argv[]) { + if (argc < 2) { + usage(); + return 1; + } + + void *handle = dlopen(argv[1], RTLD_NOW); + + if (!handle) { + PX4_ERR("%s", dlerror()); + return 1; + } + + void *main_address = dlsym(handle, "px4_module_main"); + + if (!main_address) { + PX4_ERR("%s", dlerror()); + dlclose(handle); + return 1; + } + + auto main_function = (int (*)(int, char **))main_address; + + return main_function(argc - 1, argv + 1); +} From 3c660acca83b7f29b490aa1e80c0d6a8d7ad1e5c Mon Sep 17 00:00:00 2001 From: Mara Bos Date: Fri, 23 Nov 2018 16:29:17 +0100 Subject: [PATCH 0096/1046] Add unit test for dynamically loading modules on posix. --- Tools/sitl_run.sh | 6 ++++++ cmake/configs/posix_sitl_default.cmake | 1 + platforms/posix/cmake/sitl_tests.cmake | 16 +++++++++++++++- posix-configs/SITL/init/test/test_dyn_hello | 8 ++++++++ .../posix/tests/dyn_hello/CMakeLists.txt | 8 ++++++++ src/platforms/posix/tests/dyn_hello/hello.cpp | 16 ++++++++++++++++ 6 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 posix-configs/SITL/init/test/test_dyn_hello create mode 100644 src/platforms/posix/tests/dyn_hello/CMakeLists.txt create mode 100644 src/platforms/posix/tests/dyn_hello/hello.cpp diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 65b5943c9223..83e7d1706c97 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -8,6 +8,7 @@ program="$3" model="$4" src_path="$5" build_path="$6" +# The rest of the arguments are files to copy into the working dir. echo SITL ARGS @@ -58,6 +59,11 @@ pkill -x px4_$model || true cp $src_path/Tools/posix_lldbinit $rootfs/.lldbinit cp $src_path/Tools/posix.gdbinit $rootfs/.gdbinit +shift 6 +for file in "$@"; do + cp "$file" $rootfs/ +done + SIM_PID=0 if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ] diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index a8a5cdbb96d3..7fcd64b75afd 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -55,6 +55,7 @@ set(config_module_list systemcmds/tests platforms/posix/tests/hello + platforms/posix/tests/dyn_hello platforms/posix/tests/hrt_test platforms/posix/tests/vcdev_test diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 0b36456e564c..2ef5a9722d30 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -99,6 +99,20 @@ add_test(NAME shutdown set_tests_properties(shutdown PROPERTIES PASS_REGULAR_EXPRESSION "Shutting down") sanitizer_fail_test_on_error(shutdown) +# Dynamic module loading test +add_test(NAME dyn + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh + $ + none + none + test_dyn_hello + ${PX4_SOURCE_DIR} + ${PX4_BINARY_DIR} + $ + WORKING_DIRECTORY ${SITL_WORKING_DIR}) +set_tests_properties(dyn PROPERTIES PASS_REGULAR_EXPRESSION "1: PASSED") +sanitizer_fail_test_on_error(dyn) + # run arbitrary commands set(test_cmds hello @@ -127,7 +141,7 @@ endforeach() add_custom_target(test_results COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test - DEPENDS px4 + DEPENDS px4 platforms__posix__tests__dyn_hello USES_TERMINAL COMMENT "Running tests in sitl" WORKING_DIRECTORY ${PX4_BINARY_DIR}) diff --git a/posix-configs/SITL/init/test/test_dyn_hello b/posix-configs/SITL/init/test/test_dyn_hello new file mode 100644 index 000000000000..e323fa95c27e --- /dev/null +++ b/posix-configs/SITL/init/test/test_dyn_hello @@ -0,0 +1,8 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +dyn ./platforms__posix__tests__dyn_hello.px4mod PASSED + +shutdown diff --git a/src/platforms/posix/tests/dyn_hello/CMakeLists.txt b/src/platforms/posix/tests/dyn_hello/CMakeLists.txt new file mode 100644 index 000000000000..fd1577b8db66 --- /dev/null +++ b/src/platforms/posix/tests/dyn_hello/CMakeLists.txt @@ -0,0 +1,8 @@ +px4_add_module( + MODULE platforms__posix__tests__dyn_hello + MAIN hello + SRCS + hello.cpp + DEPENDS + DYNAMIC + ) diff --git a/src/platforms/posix/tests/dyn_hello/hello.cpp b/src/platforms/posix/tests/dyn_hello/hello.cpp new file mode 100644 index 000000000000..067e28e95101 --- /dev/null +++ b/src/platforms/posix/tests/dyn_hello/hello.cpp @@ -0,0 +1,16 @@ +#include +#include + +extern "C" __EXPORT int hello_main(int argc, char *argv[]); +int hello_main(int argc, char *argv[]) +{ + PX4_INFO("Hello, I am a dynamically loaded module."); + + PX4_INFO("Argv:"); + + for (int i = 0; i < argc; ++i) { + PX4_INFO(" %d: %s", i, argv[i]); + } + + return 0; +} From d42edfc6123a0d7e459ff64bf0d5b3d9b4f75e70 Mon Sep 17 00:00:00 2001 From: PX4 Build Bot Date: Sun, 25 Nov 2018 07:37:28 -0500 Subject: [PATCH 0097/1046] Update submodule Mavlink v2.0 to latest Sun Nov 25 07:37:28 EST 2018 - Mavlink v2.0 in PX4/Firmware (d1dc8e440912e7918465163195b73531138e1e8c): https://github.com/mavlink/c_library_v2/commit/6203e2b64395cfa416b9321f633bc3256a79f722 - Mavlink v2.0 current upstream: https://github.com/mavlink/c_library_v2/commit/7c47254ddc8bf70eb5e82b4c2142a3faa4fa5780 - Changes: https://github.com/mavlink/c_library_v2/compare/6203e2b64395cfa416b9321f633bc3256a79f722...7c47254ddc8bf70eb5e82b4c2142a3faa4fa5780 7c47254 2018-11-21 PX4BuildBot - autogenerated headers for rev https://github.com/mavlink/mavlink/tree/aa72b9e9806854ee822eb2fb4c70c26216113141 --- mavlink/include/mavlink/v2.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 6203e2b64395..7c47254ddc8b 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 6203e2b64395cfa416b9321f633bc3256a79f722 +Subproject commit 7c47254ddc8bf70eb5e82b4c2142a3faa4fa5780 From 2c33a937ddf64c88784f928d615060c3fc3bc216 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 25 Nov 2018 10:29:42 -0500 Subject: [PATCH 0098/1046] update docker images to 2018-11-22 tag - keeping nuttx and armhf compile jobs on 2018-09-11 for now --- .ci/Jenkinsfile-SITL_tests | 4 ++-- .ci/Jenkinsfile-compile | 9 ++++----- .ci/Jenkinsfile-hardware | 4 ++-- Jenkinsfile | 34 +++++++++++++++++----------------- Tools/docker_run.sh | 14 +++++++------- 5 files changed, 32 insertions(+), 33 deletions(-) diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests index ab96c2706c8f..25a3aafb9cea 100644 --- a/.ci/Jenkinsfile-SITL_tests +++ b/.ci/Jenkinsfile-SITL_tests @@ -146,7 +146,7 @@ pipeline { stage('code coverage (python)') { agent { docker { - image 'px4io/px4-dev-base:2018-09-11' + image 'px4io/px4-dev-base:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw --cap-add SYS_PTRACE' } } @@ -169,7 +169,7 @@ pipeline { stage('unit tests') { agent { docker { - image 'px4io/px4-dev-base:2018-09-11' + image 'px4io/px4-dev-base:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw --cap-add SYS_PTRACE' } } diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 3f28381db437..f5a8c73282a4 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -9,12 +9,11 @@ pipeline { script { def build_nodes = [:] def docker_images = [ - arch: "px4io/px4-dev-base-archlinux:2018-09-11", + arch: "px4io/px4-dev-base-archlinux:2018-11-22", armhf: "px4io/px4-dev-armhf:2018-09-11", - base: "px4io/px4-dev-base:2018-09-11", - clang: "px4io/px4-dev-clang:2018-09-11", + base: "px4io/px4-dev-base:2018-11-22", nuttx: "px4io/px4-dev-nuttx:2018-09-11", - ros: "px4io/px4-dev-ros:2018-09-11", + ros: "px4io/px4-dev-ros:2018-11-22", rpi: "px4io/px4-dev-raspi:2018-09-11", snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12" ] @@ -135,7 +134,7 @@ pipeline { // TODO: actually upload artifacts to S3 stage('S3 Upload') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } options { skipDefaultCheckout() diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index ff94a7b4e3f3..5825efe7c7cc 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -9,7 +9,7 @@ pipeline { stage('px4fmu-v2') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -34,7 +34,7 @@ pipeline { stage('px4fmu-v4 (stackcheck)') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } diff --git a/Jenkinsfile b/Jenkinsfile index 1d3ed8113bd2..c676a55c3d07 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -9,7 +9,7 @@ pipeline { stage('Catkin') { agent { docker { - image 'px4io/px4-dev-ros:2018-09-24' + image 'px4io/px4-dev-ros:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -38,7 +38,7 @@ pipeline { stage('Style check') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh 'make check_format' @@ -53,7 +53,7 @@ pipeline { stage('Bloaty px4fmu-v2') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -81,7 +81,7 @@ pipeline { stage('Bloaty px4fmu-v5') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -109,7 +109,7 @@ pipeline { stage('Clang analyzer') { agent { docker { - image 'px4io/px4-dev-clang:2018-09-11' + image 'px4io/px4-dev-clang:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -167,7 +167,7 @@ pipeline { stage('Cppcheck') { agent { docker { - image 'px4io/px4-dev-base:2018-09-11' + image 'px4io/px4-dev-base:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -204,7 +204,7 @@ pipeline { stage('Check stack') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -223,7 +223,7 @@ pipeline { stage('ShellCheck') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -242,7 +242,7 @@ pipeline { stage('Module config validation') { agent { docker { - image 'px4io/px4-dev-base:2018-09-11' + image 'px4io/px4-dev-base:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -267,7 +267,7 @@ pipeline { stage('Airframe') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh 'make distclean' @@ -286,7 +286,7 @@ pipeline { stage('Parameter') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh 'make distclean' @@ -305,7 +305,7 @@ pipeline { stage('Module') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh 'make distclean' @@ -325,7 +325,7 @@ pipeline { stage('uORB graphs') { agent { docker { - image 'px4io/px4-dev-nuttx:2018-09-11' + image 'px4io/px4-dev-nuttx:2018-11-22' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -354,7 +354,7 @@ pipeline { stage('Devguide') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh('export') @@ -383,7 +383,7 @@ pipeline { stage('Userguide') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh('export') @@ -410,7 +410,7 @@ pipeline { stage('QGroundControl') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh('export') @@ -437,7 +437,7 @@ pipeline { stage('S3') { agent { - docker { image 'px4io/px4-dev-base:2018-09-11' } + docker { image 'px4io/px4-dev-base:2018-11-22' } } steps { sh('export') diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 12aee5e61938..f69da86e1daf 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -4,25 +4,25 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then echo "guessing PX4_DOCKER_REPO based on input"; if [[ $@ =~ .*px4fmu.* ]]; then # nuttx-px4fmu-v{1,2,3,4,5} - PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2018-11-22" elif [[ $@ =~ .*rpi.* ]] || [[ $@ =~ .*bebop.* ]]; then # posix_rpi_cross, posix_bebop_default - PX4_DOCKER_REPO="px4io/px4-dev-raspi:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-raspi:2018-11-22" elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then # eagle, excelsior PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2018-09-12" elif [[ $@ =~ .*ocpoc.* ]]; then # posix_ocpoc_ubuntu - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2018-11-22" elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then # clang tools - PX4_DOCKER_REPO="px4io/px4-dev-clang:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-clang:2018-11-22" elif [[ $@ =~ .*cppcheck.* ]]; then # TODO: remove this once px4io/px4-dev-base updates - PX4_DOCKER_REPO="px4io/px4-dev-base:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-base:2018-11-22" elif [[ $@ =~ .*tests* ]]; then # run all tests with simulation - PX4_DOCKER_REPO="px4io/px4-dev-simulation:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-simulation:2018-11-22" fi else echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'"; @@ -30,7 +30,7 @@ fi # otherwise default to nuttx if [ -z ${PX4_DOCKER_REPO+x} ]; then - PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2018-09-11" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2018-11-22" fi # docker hygiene From b9b301c38ea029c240c6523cf709d85a86b7cec5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 25 Nov 2018 10:57:24 -0500 Subject: [PATCH 0099/1046] update sitl_gazebo to latest master --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 2adc86b5bf84..7c97eeae139d 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 2adc86b5bf84543091470966caf33c3b4f3ee666 +Subproject commit 7c97eeae139d86b3747ea877d8067fb3180dc4f1 From ddb4b27fbec1d0cc5a5c7cc07e64bcbae6333c4c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sun, 25 Nov 2018 18:02:35 +0000 Subject: [PATCH 0100/1046] set CPACK_DEBIAN_PACKAGE_SHLIBDEPS to OFF --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fbe5a4cb64a2..b1e00ca630a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -521,7 +521,7 @@ add_custom_target(all_metadata set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${CONFIG}) set(CPACK_PACKAGE_VERSION ${PX4_GIT_TAG}) set(CPACK_PACKAGE_CONTACT ${package-contact}) -set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON) +set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS OFF) set(CPACK_DEBIAN_PACKAGE_SECTION "devel") set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional") set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "The PX4 Pro autopilot.") From 12681096906288923c70bc15ca7244e6a0df2646 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sun, 25 Nov 2018 18:09:15 +0000 Subject: [PATCH 0101/1046] bump px4-dev-raspi container tag --- .ci/Jenkinsfile-compile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index f5a8c73282a4..a0c90fdc89d9 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -14,7 +14,7 @@ pipeline { base: "px4io/px4-dev-base:2018-11-22", nuttx: "px4io/px4-dev-nuttx:2018-09-11", ros: "px4io/px4-dev-ros:2018-11-22", - rpi: "px4io/px4-dev-raspi:2018-09-11", + rpi: "px4io/px4-dev-raspi:2018-11-22", snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12" ] From eb8a3de987fd17cd50eb46efe74adb61ba807a94 Mon Sep 17 00:00:00 2001 From: CUAVcaijie Date: Mon, 26 Nov 2018 02:40:51 +0800 Subject: [PATCH 0102/1046] add rgbled_ncp5623c driver (#10889) Co-Authored-By: CUAVcaijie --- ROMFS/px4fmu_common/init.d/rcS | 2 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + .../configs/nuttx_px4fmu-v4pro_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v5_default.cmake | 1 + src/drivers/drv_led.h | 1 + src/drivers/rgbled_ncp5623c/CMakeLists.txt | 43 ++ .../rgbled_ncp5623c/rgbled_ncp5623c.cpp | 427 ++++++++++++++++++ .../rgbled_ncp5623c/rgbled_ncp5623c_params.c | 56 +++ 9 files changed, 533 insertions(+) create mode 100755 src/drivers/rgbled_ncp5623c/CMakeLists.txt create mode 100755 src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp create mode 100755 src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd59fe865ee0..02595c3ee3a0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -243,6 +243,8 @@ else fi fi + rgbled_ncp5623c start + # # FMUv5 may have both PWM I2C RGB LED support. # diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index afbd4e3c6b6d..fadda7886589 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -46,6 +46,7 @@ set(config_module_list drivers/px4fmu drivers/px4io drivers/rgbled + drivers/rgbled_ncp5623c drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index e618b027fd32..56cdb9dfc70f 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -34,6 +34,7 @@ set(config_module_list drivers/px4flow drivers/px4fmu drivers/rgbled + drivers/rgbled_ncp5623c drivers/rc_input drivers/stm32 drivers/stm32/adc diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index 0407747ab966..02fd61d13e8d 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -39,6 +39,7 @@ set(config_module_list drivers/px4fmu drivers/px4io drivers/rgbled + drivers/rgbled_ncp5623c drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 97cda9527343..49c991808e29 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -40,6 +40,7 @@ set(config_module_list drivers/px4io drivers/rc_input drivers/rgbled + drivers/rgbled_ncp5623c #drivers/rgbled_pwm # Enable to put the three leds into PWM RGB mode drivers/stm32 drivers/stm32/adc diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 4cec85f71e05..e02bb4fa2448 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -59,4 +59,5 @@ static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS; // Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist #define RGBLED0_DEVICE_PATH "/dev/rgbled0" // Primary RGB LED on i2c +#define RGBLED1_DEVICE_PATH "/dev/rgbled1" // Primary RGB LED(NCP5623C) on i2c #define RGBLED_PWM0_DEVICE_PATH "/dev/rgbled_pwm0" // Secondary RGB LED on PWM diff --git a/src/drivers/rgbled_ncp5623c/CMakeLists.txt b/src/drivers/rgbled_ncp5623c/CMakeLists.txt new file mode 100755 index 000000000000..76710a0b01de --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__rgbled_ncp5623c + MAIN rgbled_ncp5623c + COMPILE_FLAGS + SRCS + rgbled_ncp5623c.cpp + DEPENDS + drivers__device + led + ) diff --git a/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp new file mode 100755 index 000000000000..0dec360493b7 --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -0,0 +1,427 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rgbled_ncp5623c.cpp + * + * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C. + * + * @author CUAVcaijie + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include + +#include "uORB/topics/parameter_update.h" + +#define ADDR 0x39 /**< I2C adress of NCP5623C */ + +#define NCP5623_LED_CURRENT 0x20 /**< Current register */ +#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ +#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ +#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ + +#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ +#define NCP5623_LED_OFF 0x00 /**< off */ + + +class RGBLED_NPC5623C : public device::I2C +{ +public: + RGBLED_NPC5623C(int bus, int rgbled); + virtual ~RGBLED_NPC5623C(); + + + virtual int init(); + virtual int probe(); +private: + work_s _work; + + float _brightness; + float _max_brightness; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + volatile bool _running; + volatile bool _should_run; + bool _leds_enabled; + int _param_sub; + + LedController _led_controller; + + static void led_trampoline(void *arg); + void led(); + + int send_led_rgb(); + void update_params(); + + int write(uint8_t reg, uint8_t data); +}; + +/* for now, we only support one RGBLED */ +namespace +{ +RGBLED_NPC5623C *g_rgbled = nullptr; +} + +void rgbled_ncp5623c_usage(); + +extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]); + +RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) : + I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled +#ifdef __PX4_NUTTX + , 100000 /* maximum speed supported */ +#endif + ), + _work{}, + _brightness(1.0f), + _max_brightness(1.0f), + _r(0), + _g(0), + _b(0), + _running(false), + _should_run(true), + _leds_enabled(true), + _param_sub(-1) +{ +} + +RGBLED_NPC5623C::~RGBLED_NPC5623C() +{ + _should_run = false; + int counter = 0; + + while (_running && ++counter < 10) { + usleep(100000); + } +} + +int +RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) +{ + uint8_t msg[1] = { 0x00 }; + msg[0] = ((reg & 0xe0) | (data & 0x1f)); + + int ret = transfer(&msg[0], 1, nullptr, 0); + + return ret; +} + +int +RGBLED_NPC5623C::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + update_params(); + + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this, 0); + + return OK; +} + +int +RGBLED_NPC5623C::probe() +{ + _retries = 4; + + return write(NCP5623_LED_CURRENT, 0x00); +} + +void +RGBLED_NPC5623C::led_trampoline(void *arg) +{ + RGBLED_NPC5623C *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLED_NPC5623C::led() +{ + if (!_should_run) { + if (_param_sub >= 0) { + orb_unsubscribe(_param_sub); + } + + int led_control_sub = _led_controller.led_control_subscription(); + + if (led_control_sub >= 0) { + orb_unsubscribe(led_control_sub); + } + + _running = false; + return; + } + + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (!_led_controller.is_init()) { + int led_control_sub = orb_subscribe(ORB_ID(led_control)); + _led_controller.init(led_control_sub); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + + LedControlData led_control_data; + + if (_led_controller.update(led_control_data) == 1) { + switch (led_control_data.leds[0].color) { + case led_control_s::COLOR_RED: + _r = NCP5623_LED_BRIGHT; _g = 0; _b = 0; + break; + + case led_control_s::COLOR_GREEN: + _r = 0; _g = NCP5623_LED_BRIGHT; _b = 0; + break; + + case led_control_s::COLOR_BLUE: + _r = 0; _g = 0; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_AMBER: //make it the same as yellow + case led_control_s::COLOR_YELLOW: + _r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = 0; + break; + + case led_control_s::COLOR_PURPLE: + _r = NCP5623_LED_BRIGHT; _g = 0; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_CYAN: + _r = 0; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_WHITE: + _r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT; + break; + + default: // led_control_s::COLOR_OFF + _r = 0; _g = 0; _b = 0; + break; + } + + _brightness = (float)led_control_data.leds[0].brightness / 255.f; + send_led_rgb(); + + } + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this, + USEC2TICK(_led_controller.maximum_update_interval())); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED_NPC5623C::send_led_rgb() +{ + + uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80}; + uint8_t brightness = 0x1f * _max_brightness; + + msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f); + msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f); + msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f); + msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f); + + return transfer(&msg[0], 7, nullptr, 0); +} + +void +RGBLED_NPC5623C::update_params() +{ + int32_t maxbrt = 31; + param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 31 ? 31 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + if (maxbrt == 0) { + maxbrt = 1; + } + + _max_brightness = maxbrt / 31.0f; +} + +void +rgbled_ncp5623c_usage() +{ + PX4_INFO("missing command: try 'start', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + PX4_INFO(" -a addr (0x%x)", ADDR); +} + +int +rgbled_ncp5623c_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(myoptarg, nullptr, 0); + break; + + case 'b': + i2cdevice = strtol(myoptarg, nullptr, 0); + break; + + default: + rgbled_ncp5623c_usage(); + return 1; + } + } + + if (myoptind >= argc) { + rgbled_ncp5623c_usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + PX4_WARN("already started"); + return 1; + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED_NPC5623C(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED_NPC5623C(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + } + + return 0; + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + PX4_WARN("not started"); + rgbled_ncp5623c_usage(); + return 1; + } + + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + return 0; + } + + rgbled_ncp5623c_usage(); + return 1; +} diff --git a/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c new file mode 100755 index 000000000000..d1eb461e4ab2 --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file rgbled_ncp5623c_params.c + * + * Parameters defined by the RBG led driver + * + * @author CUAVcaijie + */ + + +#include +#include + +/** + * RGB Led brightness limit + * + * Set to 0 to disable, 1 for minimum brightness up to 31 (max) + * + * @min 0 + * @max 31 + * @group System + */ +PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31); + From bef8b6b94996b3528630e3ec19a110df3e849765 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sun, 25 Nov 2018 20:01:23 +0100 Subject: [PATCH 0103/1046] mavlink hash check: return early in case of hash check parameter (#10861) * a request for setting the '_HASH_CHECK' parameter is handled differently than the same request for a standard parameter. Make sure we don't actually try to set the parameter. Signed-off-by: Roman --- src/modules/mavlink/mavlink_parameters.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b853648c2256..cbfdf75f2d5f 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -134,8 +134,12 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* Whatever the value is, we're being told to stop sending */ - if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0 && _mavlink->hash_check_enabled()) { - _send_all_index = -1; + if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { + + if (_mavlink->hash_check_enabled()) { + _send_all_index = -1; + } + /* No other action taken, return */ return; } From a4c4f356b3ea0d6d71246cfa66058ca8b3c68888 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Nov 2018 15:00:28 -0500 Subject: [PATCH 0104/1046] Makefile handle spaces in path --- Makefile | 80 ++++++++++++++++++++++++++++---------------------------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/Makefile b/Makefile index a25bc49b12ee..2be3424dc500 100644 --- a/Makefile +++ b/Makefile @@ -98,7 +98,7 @@ else PX4_MAKE_ARGS = -j$(j) --no-print-directory endif -SRC_DIR := $(shell dirname $(realpath $(lastword $(MAKEFILE_LIST)))) +SRC_DIR := $(shell dirname "$(realpath $(lastword $(MAKEFILE_LIST)))") # check if replay env variable is set & set build dir accordingly ifdef replay @@ -143,9 +143,9 @@ endif # describe how to build a cmake config define cmake-build +@$(eval PX4_CONFIG = $(1)) -+@$(eval BUILD_DIR = $(SRC_DIR)/build/$(PX4_CONFIG)$(BUILD_DIR_SUFFIX)) ++@$(eval BUILD_DIR = "$(SRC_DIR)"/build/$(PX4_CONFIG)$(BUILD_DIR_SUFFIX)) +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(BUILD_DIR)/Makefile ]; then rm -rf $(BUILD_DIR); fi -+@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake $(SRC_DIR) -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) -DCONFIG=$(PX4_CONFIG) || (rm -rf $(BUILD_DIR)); fi ++@if [ ! -e $(BUILD_DIR)/CMakeCache.txt ]; then mkdir -p $(BUILD_DIR) && cd $(BUILD_DIR) && cmake "$(SRC_DIR)" -G"$(PX4_CMAKE_GENERATOR)" $(CMAKE_ARGS) -DCONFIG=$(PX4_CONFIG) || (rm -rf $(BUILD_DIR)); fi +@$(PX4_MAKE) -C $(BUILD_DIR) $(PX4_MAKE_ARGS) $(ARGS) endef @@ -157,7 +157,7 @@ define colorecho endef # Get a list of all config targets cmake/configs/*.cmake -ALL_CONFIG_TARGETS := $(basename $(shell find "$(SRC_DIR)/cmake/configs" -maxdepth 1 ! -name '*_common*' ! -name '*_sdflight_*' -name '*.cmake' -print | sed -e 's:^.*/::' | sort)) +ALL_CONFIG_TARGETS := $(basename $(shell find "$(SRC_DIR)"/cmake/configs -maxdepth 1 ! -name '*_common*' ! -name '*_sdflight_*' -name '*.cmake' -print | sed -e 's:^.*/::' | sort)) # Strip off leading nuttx_ NUTTX_CONFIG_TARGETS := $(patsubst nuttx_%,%,$(filter nuttx_%,$(ALL_CONFIG_TARGETS))) @@ -285,10 +285,10 @@ module_documentation: px4_metadata: parameters_metadata airframe_metadata module_documentation doxygen: - @mkdir -p $(SRC_DIR)/build/doxygen - @cd $(SRC_DIR)/build/doxygen && cmake $(SRC_DIR) $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DBUILD_DOXYGEN=ON - @$(PX4_MAKE) -C $(SRC_DIR)/build/doxygen - @touch $(SRC_DIR)/build/doxygen/Documentation/.nojekyll + @mkdir -p "$(SRC_DIR)"/build/doxygen + @cd "$(SRC_DIR)"/build/doxygen && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DBUILD_DOXYGEN=ON + @$(PX4_MAKE) -C "$(SRC_DIR)"/build/doxygen + @touch "$(SRC_DIR)"/build/doxygen/Documentation/.nojekyll # Astyle # -------------------------------------------------------------------- @@ -296,12 +296,12 @@ doxygen: check_format: $(call colorecho,'Checking formatting with astyle') - @$(SRC_DIR)/Tools/astyle/check_code_style_all.sh - @cd $(SRC_DIR) && git diff --check + @"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh + @cd "$(SRC_DIR)" && git diff --check format: $(call colorecho,'Formatting with astyle') - @$(SRC_DIR)/Tools/astyle/check_code_style_all.sh --fix + @"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix # Testing # -------------------------------------------------------------------- @@ -315,32 +315,32 @@ tests: tests_coverage: @$(MAKE) clean @$(MAKE) --no-print-directory posix_sitl_default test_coverage_genhtml PX4_CMAKE_BUILD_TYPE=Coverage - @echo "Open $(SRC_DIR)/build/posix_sitl_default/coverage-html/index.html to see coverage" + @echo "Open "$(SRC_DIR)"/build/posix_sitl_default/coverage-html/index.html to see coverage" rostest: posix_sitl_default @$(MAKE) --no-print-directory posix_sitl_default sitl_gazebo tests_mission: rostest - @$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_missions.test + @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_missions.test tests_mission_coverage: @$(MAKE) clean @$(MAKE) --no-print-directory posix_sitl_default PX4_CMAKE_BUILD_TYPE=Coverage @$(MAKE) --no-print-directory posix_sitl_default sitl_gazebo PX4_CMAKE_BUILD_TYPE=Coverage - @$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=VTOL_mission_1 vehicle:=standard_vtol + @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=VTOL_mission_1 vehicle:=standard_vtol @$(MAKE) --no-print-directory posix_sitl_default generate_coverage tests_offboard: rostest - @$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test - @$(SRC_DIR)/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test + @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test + @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test python_coverage: - @mkdir -p $(SRC_DIR)/build/python_coverage - @cd $(SRC_DIR)/build/python_coverage && cmake $(SRC_DIR) $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DPYTHON_COVERAGE=ON - @$(PX4_MAKE) -C $(SRC_DIR)/build/python_coverage - @$(PX4_MAKE) -C $(SRC_DIR)/build/python_coverage metadata_airframes - @$(PX4_MAKE) -C $(SRC_DIR)/build/python_coverage metadata_parameters - #@$(PX4_MAKE) -C $(SRC_DIR)/build/python_coverage module_documentation # TODO: fix within coverage.py + @mkdir -p "$(SRC_DIR)"/build/python_coverage + @cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DPYTHON_COVERAGE=ON + @$(PX4_MAKE) -C "$(SRC_DIR)"/build/python_coverage + @$(PX4_MAKE) -C "$(SRC_DIR)"/build/python_coverage metadata_airframes + @$(PX4_MAKE) -C "$(SRC_DIR)"/build/python_coverage metadata_parameters + #@$(PX4_MAKE) -C "$(SRC_DIR)"/build/python_coverage module_documentation # TODO: fix within coverage.py @coverage combine `find . -name .coverage\*` @coverage report -m @@ -352,49 +352,49 @@ python_coverage: scan-build: @export CCC_CC=clang @export CCC_CXX=clang++ - @rm -rf $(SRC_DIR)/build/posix_sitl_default-scan-build - @rm -rf $(SRC_DIR)/build/scan-build/report_latest - @mkdir -p $(SRC_DIR)/build/posix_sitl_default-scan-build - @cd $(SRC_DIR)/build/posix_sitl_default-scan-build && scan-build cmake $(SRC_DIR) -GNinja -DCONFIG=posix_sitl_default - @scan-build -o $(SRC_DIR)/build/scan-build cmake --build $(SRC_DIR)/build/posix_sitl_default-scan-build - @find $(SRC_DIR)/build/scan-build -maxdepth 1 -mindepth 1 -type d -exec cp -r "{}" $(SRC_DIR)/build/scan-build/report_latest \; + @rm -rf "$(SRC_DIR)"/build/posix_sitl_default-scan-build + @rm -rf "$(SRC_DIR)"/build/scan-build/report_latest + @mkdir -p "$(SRC_DIR)"/build/posix_sitl_default-scan-build + @cd "$(SRC_DIR)"/build/posix_sitl_default-scan-build && scan-build cmake "$(SRC_DIR)" -GNinja -DCONFIG=posix_sitl_default + @scan-build -o "$(SRC_DIR)"/build/scan-build cmake --build "$(SRC_DIR)"/build/posix_sitl_default-scan-build + @find "$(SRC_DIR)"/build/scan-build -maxdepth 1 -mindepth 1 -type d -exec cp -r "{}" "$(SRC_DIR)"/build/scan-build/report_latest \; posix_sitl_default-clang: - @mkdir -p $(SRC_DIR)/build/posix_sitl_default-clang - @cd $(SRC_DIR)/build/posix_sitl_default-clang && cmake $(SRC_DIR) $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++ - @$(PX4_MAKE) -C $(SRC_DIR)/build/posix_sitl_default-clang + @mkdir -p "$(SRC_DIR)"/build/posix_sitl_default-clang + @cd "$(SRC_DIR)"/build/posix_sitl_default-clang && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=posix_sitl_default -DCMAKE_C_COMPILER=clang -DCMAKE_CXX_COMPILER=clang++ + @$(PX4_MAKE) -C "$(SRC_DIR)"/build/posix_sitl_default-clang clang-tidy: posix_sitl_default-clang - @cd $(SRC_DIR)/build/posix_sitl_default-clang && $(SRC_DIR)/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -p . + @cd "$(SRC_DIR)"/build/posix_sitl_default-clang && "$(SRC_DIR)"/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -p . # to automatically fix a single check at a time, eg modernize-redundant-void-arg # % run-clang-tidy-4.0.py -fix -j4 -checks=-\*,modernize-redundant-void-arg -p . clang-tidy-fix: posix_sitl_default-clang - @cd $(SRC_DIR)/build/posix_sitl_default-clang && $(SRC_DIR)/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -fix -p . + @cd "$(SRC_DIR)"/build/posix_sitl_default-clang && "$(SRC_DIR)"/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -fix -p . # modified version of run-clang-tidy.py to return error codes and only output relevant results clang-tidy-quiet: posix_sitl_default-clang - @cd $(SRC_DIR)/build/posix_sitl_default-clang && $(SRC_DIR)/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -p . + @cd "$(SRC_DIR)"/build/posix_sitl_default-clang && "$(SRC_DIR)"/Tools/run-clang-tidy.py -header-filter=".*\.hpp" -j$(j) -p . # TODO: Fix cppcheck errors then try --enable=warning,performance,portability,style,unusedFunction or --enable=all cppcheck: posix_sitl_default - @mkdir -p $(SRC_DIR)/build/cppcheck - @cppcheck -i$(SRC_DIR)/src/examples --enable=performance --std=c++11 --std=c99 --std=posix --project=$(SRC_DIR)/build/posix_sitl_default/compile_commands.json --xml-version=2 2> $(SRC_DIR)/build/cppcheck/cppcheck-result.xml > /dev/null - @cppcheck-htmlreport --source-encoding=ascii --file=$(SRC_DIR)/build/cppcheck/cppcheck-result.xml --report-dir=$(SRC_DIR)/build/cppcheck --source-dir=$(SRC_DIR)/src/ + @mkdir -p "$(SRC_DIR)"/build/cppcheck + @cppcheck -i"$(SRC_DIR)"/src/examples --enable=performance --std=c++11 --std=c99 --std=posix --project="$(SRC_DIR)"/build/posix_sitl_default/compile_commands.json --xml-version=2 2> "$(SRC_DIR)"/build/cppcheck/cppcheck-result.xml > /dev/null + @cppcheck-htmlreport --source-encoding=ascii --file="$(SRC_DIR)"/build/cppcheck/cppcheck-result.xml --report-dir="$(SRC_DIR)"/build/cppcheck --source-dir="$(SRC_DIR)"/src/ shellcheck_all: - @$(SRC_DIR)/Tools/run-shellcheck.sh $(SRC_DIR)/ROMFS/px4fmu_common/ + @"$(SRC_DIR)"/Tools/run-shellcheck.sh "$(SRC_DIR)"/ROMFS/px4fmu_common/ @make px4fmu-v2_default shellcheck validate_module_configs: - @find $(SRC_DIR)/src/modules $(SRC_DIR)/src/drivers $(SRC_DIR)/src/lib -name *.yaml -type f -print0 | xargs -0 $(SRC_DIR)/Tools/validate_yaml.py --schema-file $(SRC_DIR)/validation/module_schema.yaml + @find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f -print0 | xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml # Cleanup # -------------------------------------------------------------------- .PHONY: clean submodulesclean submodulesupdate gazeboclean distclean clean: - @rm -rf $(SRC_DIR)/build + @rm -rf "$(SRC_DIR)"/build submodulesclean: @git submodule foreach --quiet --recursive git clean -ff -x -d From f387fe7e65ff0d358c6809baa928e6a2df660ae5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ram=C3=B3n=20Hern=C3=A1n=20Roche=20Quintana?= Date: Fri, 23 Nov 2018 09:55:44 -0800 Subject: [PATCH 0105/1046] sitl_run.sh handle spaces in paths launch script for jmavsim failed to start if there were any spaces in paths, string handling in paths was improved and tested. --- Tools/sitl_run.sh | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 83e7d1706c97..885213b8cdaf 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -56,8 +56,8 @@ pkill -x gazebo || true pkill -x px4 || true pkill -x px4_$model || true -cp $src_path/Tools/posix_lldbinit $rootfs/.lldbinit -cp $src_path/Tools/posix.gdbinit $rootfs/.gdbinit +cp "$src_path/Tools/posix_lldbinit" "$rootfs/.lldbinit" +cp "$src_path/Tools/posix.gdbinit" "$rootfs/.gdbinit" shift 6 for file in "$@"; do @@ -69,7 +69,7 @@ SIM_PID=0 if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ] then # Start Java simulator - $src_path/Tools/jmavsim_run.sh -r 500 & + "$src_path"/Tools/jmavsim_run.sh -r 500 & SIM_PID=`echo $!` elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ] then @@ -78,9 +78,9 @@ then if [[ -z "$DONT_RUN" ]] then # Set the plugin path so Gazebo finds our model and sim - source $src_path/Tools/setup_gazebo.bash ${src_path} ${build_path} + source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}" - gzserver --verbose ${src_path}/Tools/sitl_gazebo/worlds/${model}.world & + gzserver --verbose "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" & SIM_PID=`echo $!` if [[ -n "$HEADLESS" ]]; then @@ -105,9 +105,9 @@ pushd "$rootfs" >/dev/null set +e if [[ ${model} == test_* ]] || [[ ${model} == *_generated ]]; then - sitl_command="$sitl_bin $no_pxh $src_path/ROMFS/px4fmu_test -s ${src_path}/posix-configs/SITL/init/test/${model} -t $src_path/test_data" + sitl_command="\"$sitl_bin\" $no_pxh \"$src_path\"/ROMFS/px4fmu_test -s \"${src_path}\"/posix-configs/SITL/init/test/${model} -t \"$src_path\"/test_data" else - sitl_command="$sitl_bin $no_pxh $src_path/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -t $src_path/test_data" + sitl_command="\"$sitl_bin\" $no_pxh \"$src_path\"/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -t \"$src_path\"/test_data" fi echo SITL COMMAND: $sitl_command From d4f713b2861efa9dce79cb814396370904d9a2ac Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Nov 2018 14:03:24 -0500 Subject: [PATCH 0106/1046] commander cleanup home position handling --- src/modules/commander/Commander.cpp | 228 +++++++++++----------------- src/modules/commander/Commander.hpp | 22 ++- src/modules/uORB/Publication.hpp | 6 + 3 files changed, 113 insertions(+), 143 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d484acf0ed0a..79f89158b344 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -156,7 +156,6 @@ static struct actuator_armed_s armed = {}; static struct safety_s safety = {}; static struct vehicle_control_mode_s control_mode = {}; static struct offboard_control_mode_s offboard_control_mode = {}; -static struct home_position_s _home = {}; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; static struct commander_state_s internal_state = {}; @@ -500,19 +499,9 @@ void usage(const char *reason) void print_status() { - PX4_INFO("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); - PX4_INFO("safety: USB enabled: %s, power state valid: %s", (status_flags.usb_connected) ? "[OK]" : "[NO]", - (status_flags.condition_power_input_valid) ? " [OK]" : "[NO]"); - PX4_INFO("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); - PX4_INFO("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); - PX4_INFO("datalink: %s %s", (status.data_link_lost) ? "LOST" : "OK", (status.high_latency_data_link_active) ? "(high latency)" : " "); - PX4_INFO("main state: %d", internal_state.main_state); - PX4_INFO("nav state: %d", status.nav_state); PX4_INFO("arming: %s", arming_state_names[status.arming_state]); } -static orb_advert_t status_pub; - transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -546,9 +535,14 @@ Commander::Commander() : _battery_sub = orb_subscribe(ORB_ID(battery_status)); } +Commander::~Commander() +{ + orb_unsubscribe(_battery_sub); +} + bool Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local, - home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed) + orb_advert_t *command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) @@ -760,10 +754,9 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ if (cmd_arms && (arming_res == TRANSITION_CHANGED) && - (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && - !home->manual_home) { + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) { - set_home_position(*home_pub, *home, false); + set_home_position(); } } } @@ -800,7 +793,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ if (use_current) { /* use current position */ - if (set_home_position(*home_pub, *home, false)) { + if (set_home_position()) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -817,32 +810,25 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ if (local_pos.xy_global && local_pos.z_global) { /* use specified position */ - home->timestamp = hrt_absolute_time(); + home_position_s home{}; + home.timestamp = hrt_absolute_time(); - home->lat = lat; - home->lon = lon; - home->alt = alt; + home.lat = lat; + home.lon = lon; + home.alt = alt; - home->manual_home = true; - home->valid_alt = true; - home->valid_hpos = true; + home.manual_home = true; + home.valid_alt = true; + home.valid_hpos = true; // update local projection reference including altitude struct map_projection_reference_s ref_pos; map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon); - map_projection_project(&ref_pos, lat, lon, &home->x, &home->y); - home->z = -(alt - local_pos.ref_alt); - - /* announce new home position */ - if (*home_pub != nullptr) { - orb_publish(ORB_ID(home_position), *home_pub, home); - - } else { - *home_pub = orb_advertise(ORB_ID(home_position), home); - } + map_projection_project(&ref_pos, lat, lon, &home.x, &home.y); + home.z = -(alt - local_pos.ref_alt); /* mark home position as set */ - status_flags.condition_home_position_valid = true; + status_flags.condition_home_position_valid = _home_pub.update(home); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1041,65 +1027,69 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ * time the vehicle is armed with a good GPS fix. **/ bool -Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref) +Commander::set_home_position() { - const vehicle_local_position_s &localPosition = _local_position_sub.get(); - const vehicle_global_position_s &globalPosition = _global_position_sub.get(); + // Need global and local position fix to be able to set home + if (status_flags.condition_global_position_valid && status_flags.condition_local_position_valid) { - if (!set_alt_only_to_lpos_ref) { - //Need global and local position fix to be able to set home - if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) { - return false; - } + const vehicle_global_position_s &gpos = _global_position_sub.get(); - //Ensure that the GPS accuracy is good enough for intializing home - if (globalPosition.eph > _home_eph_threshold.get() || globalPosition.epv > _home_epv_threshold.get()) { - return false; - } + // Ensure that the GPS accuracy is good enough for intializing home + if ((gpos.eph <= _home_eph_threshold.get()) && (gpos.epv <= _home_epv_threshold.get())) { - // Set home position - home.lat = globalPosition.lat; - home.lon = globalPosition.lon; - home.valid_hpos = true; + const vehicle_local_position_s &lpos = _local_position_sub.get(); - home.alt = globalPosition.alt; - home.valid_alt = true; + // Set home position + home_position_s &home = _home_pub.get(); - home.x = localPosition.x; - home.y = localPosition.y; - home.z = localPosition.z; + home.lat = gpos.lat; + home.lon = gpos.lon; + home.valid_hpos = true; - home.yaw = localPosition.yaw; + home.alt = gpos.alt; + home.valid_alt = true; - //Play tune first time we initialize HOME - if (!status_flags.condition_home_position_valid) { - tune_home_set(true); - } + home.x = lpos.x; + home.y = lpos.y; + home.z = lpos.z; - /* mark home position as set */ - status_flags.condition_home_position_valid = true; + home.yaw = lpos.yaw; - } else if (!home.valid_alt && localPosition.z_global) { - // handle special case where we are setting only altitude using local position reference - home.alt = localPosition.ref_alt; - home.valid_alt = true; + //Play tune first time we initialize HOME + if (!status_flags.condition_home_position_valid) { + tune_home_set(true); + } - } else { - return false; + /* mark home position as set */ + status_flags.condition_home_position_valid = _home_pub.update(); + + home.timestamp = hrt_absolute_time(); + home.manual_home = false; + + return status_flags.condition_home_position_valid; + } } - home.timestamp = hrt_absolute_time(); - home.manual_home = false; + return false; +} - /* announce new home position */ - if (homePub != nullptr) { - orb_publish(ORB_ID(home_position), homePub, &home); +bool +Commander::set_home_position_alt_only() +{ + const vehicle_local_position_s &lpos = _local_position_sub.get(); - } else { - homePub = orb_advertise(ORB_ID(home_position), &home); + if (!_home_pub.get().valid_alt && lpos.z_global) { + // handle special case where we are setting only altitude using local position reference + home_position_s home{}; + home.alt = lpos.ref_alt; + home.valid_alt = true; + + home.timestamp = hrt_absolute_time(); + + return _home_pub.update(home); } - return true; + return false; } void @@ -1180,50 +1170,28 @@ Commander::run() } // We want to accept RC inputs as default - status_flags.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL; internal_state.timestamp = hrt_absolute_time(); status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; - status.failsafe = false; - - /* neither manual nor offboard control commands have been received */ - status_flags.offboard_control_signal_found_once = false; - status_flags.rc_signal_found_once = false; - /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status_flags.offboard_control_signal_lost = true; status.data_link_lost = true; - status_flags.offboard_control_loss_timeout = false; - - status_flags.condition_system_hotplug_timeout = false; status.timestamp = hrt_absolute_time(); status_flags.condition_power_input_valid = true; - status_flags.usb_connected = false; status_flags.rc_calibration_valid = true; - // CIRCUIT BREAKERS - status_flags.circuit_breaker_engaged_power_check = false; - status_flags.circuit_breaker_engaged_airspd_check = false; - status_flags.circuit_breaker_engaged_enginefailure_check = false; - status_flags.circuit_breaker_engaged_gpsfailure_check = false; get_circuit_breaker_params(); - /* Set position and velocity validty to false */ - status_flags.condition_global_position_valid = false; - status_flags.condition_local_position_valid = false; - status_flags.condition_local_velocity_valid = false; - status_flags.condition_local_altitude_valid = false; - /* publish initial state */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + _status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - if (status_pub == nullptr) { + if (_status_pub == nullptr) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); px4_task_exit(PX4_ERROR); @@ -1239,10 +1207,6 @@ Commander::run() memset(&control_mode, 0, sizeof(control_mode)); orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - /* home position */ - orb_advert_t home_pub = nullptr; - memset(&_home, 0, sizeof(_home)); - /* command ack */ orb_advert_t command_ack_pub = nullptr; orb_advert_t commander_state_pub = nullptr; @@ -1379,7 +1343,6 @@ Commander::run() uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ /* check which state machines for changes, clear "changed" flag */ - bool main_state_changed = false; bool failsafe_old = false; bool have_taken_off_since_arming = false; @@ -2080,7 +2043,7 @@ Commander::run() /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED || first_rc_eval) { tune_positive(armed.armed); - main_state_changed = true; + status_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -2207,7 +2170,7 @@ Commander::run() orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, cmd, &armed, &_home, &home_pub, &command_ack_pub, &status_changed)) { + if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) { status_changed = true; } } @@ -2296,7 +2259,7 @@ Commander::run() const hrt_abstime now = hrt_absolute_time(); // automatically set or update home position - if (!_home.manual_home) { + if (!_home_pub.get().manual_home) { const vehicle_local_position_s &local_position = _local_position_sub.get(); if (armed.armed) { @@ -2304,7 +2267,7 @@ Commander::run() (hrt_elapsed_time(&commander_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - set_home_position(home_pub, _home, false); + set_home_position(); } } else { @@ -2313,29 +2276,28 @@ Commander::run() /* distance from home */ float home_dist_xy = -1.0f; float home_dist_z = -1.0f; - mavlink_wpm_distance_to_point_local(_home.x, _home.y, _home.z, + mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z, local_position.x, local_position.y, local_position.z, &home_dist_xy, &home_dist_z); - if ((home_dist_xy > local_position.eph * 2) || (home_dist_z > local_position.epv * 2)) { + if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) { /* update when disarmed, landed and moved away from current home position */ - set_home_position(home_pub, _home, false); + set_home_position(); } } } else { /* First time home position update - but only if disarmed */ - set_home_position(home_pub, _home, false); - } - } - - /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. - * This allows home altitude to be used in the calculation of height above takeoff location when GPS - * use has commenced after takeoff. */ - if (!_home.valid_alt && local_position.z_global) { - set_home_position(home_pub, _home, true); + set_home_position(); + /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. + * This allows home altitude to be used in the calculation of height above takeoff location when GPS + * use has commenced after takeoff. */ + if (!status_flags.condition_home_position_valid) { + set_home_position_alt_only(); + } + } } } @@ -2381,21 +2343,15 @@ Commander::run() failsafe_old = status.failsafe; } - // TODO handle mode changes by commands - if (main_state_changed || nav_state_changed) { - status_changed = true; - main_state_changed = false; - } - /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 1 Hz or immediately when changed */ - if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed) { + if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed || nav_state_changed) { set_control_mode(); control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); status.timestamp = now; - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + orb_publish(ORB_ID(vehicle_status), _status_pub, &status); armed.timestamp = now; @@ -2523,13 +2479,13 @@ Commander::run() /* close fds */ led_deinit(); buzzer_deinit(); - px4_close(sp_man_sub); - px4_close(offboard_control_mode_sub); - px4_close(safety_sub); - px4_close(cmd_sub); - px4_close(subsys_sub); - px4_close(param_changed_sub); - px4_close(land_detector_sub); + orb_unsubscribe(sp_man_sub); + orb_unsubscribe(offboard_control_mode_sub); + orb_unsubscribe(safety_sub); + orb_unsubscribe(cmd_sub); + orb_unsubscribe(subsys_sub); + orb_unsubscribe(param_changed_sub); + orb_unsubscribe(land_detector_sub); thread_running = false; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f51ab5a9592a..1cb4d171af77 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -37,16 +37,16 @@ #include "state_machine_helper.h" #include "failure_detector/FailureDetector.hpp" -#include +#include +#include #include #include -#include +#include // publications #include #include #include -#include #include #include #include @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ class Commander : public ModuleBase, public ModuleParams { public: Commander(); + ~Commander(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -132,10 +134,11 @@ class Commander : public ModuleBase, public ModuleParams FailureDetector _failure_detector; bool _failure_detector_termination_printed{false}; - bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, - actuator_armed_s *armed, home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed); + bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, + orb_advert_t *command_ack_pub, bool *changed); - bool set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref); + bool set_home_position(); + bool set_home_position_alt_only(); // Set the main system state based on RC and override device inputs transition_result_t set_main_state(const vehicle_status_s &status, bool *changed); @@ -146,7 +149,8 @@ class Commander : public ModuleBase, public ModuleParams // Set the system main state based on the current RC inputs transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed); - void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed); + void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, + bool *changed); bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, @@ -192,6 +196,10 @@ class Commander : public ModuleBase, public ModuleParams Subscription _mission_result_sub{ORB_ID(mission_result)}; Subscription _global_position_sub{ORB_ID(vehicle_global_position)}; Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + + Publication _home_pub{ORB_ID(home_position)}; + + orb_advert_t _status_pub{nullptr}; }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 9aab16dda32e..c9547c7b03bb 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -158,6 +158,12 @@ class __EXPORT Publication final : public PublicationNode return PublicationBase::update((void *)(&_data)); } + bool update(const T &data) + { + _data = data; + return update(); + } + private: T _data; }; From b80fa5f2f55866e3ba45b0ecbc76873de6a97296 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 15 Nov 2018 13:00:15 -0800 Subject: [PATCH 0107/1046] px4fmu-v5:Remove old comment --- src/drivers/boards/px4fmu-v5/timer_config.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v5/timer_config.c b/src/drivers/boards/px4fmu-v5/timer_config.c index aaa9eccc8157..793f0a7b1a7f 100644 --- a/src/drivers/boards/px4fmu-v5/timer_config.c +++ b/src/drivers/boards/px4fmu-v5/timer_config.c @@ -159,16 +159,16 @@ __EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { .gpio_in = GPIO_TIM12_CH1IN, .timer_index = 2, .timer_channel = 1, - .ccr_offset = STM32_GTIM_CCR1_OFFSET, // TODO: need revision - .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF // TODO: need revision + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF }, { .gpio_out = GPIO_TIM12_CH2OUT, .gpio_in = GPIO_TIM12_CH2IN, .timer_index = 2, .timer_channel = 2, - .ccr_offset = STM32_GTIM_CCR2_OFFSET, // TODO: need revision - .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF // TODO: need revision + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF } }; From 2973b77f188b01a879ac46806cd461cef25b1a38 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 25 Nov 2018 16:25:02 +0100 Subject: [PATCH 0108/1046] appveyor: use new 0.5 installer --- appveyor.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 395c8e695b0e..8ff7a866eac3 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -22,7 +22,7 @@ install: # if the toolchain wasn't restored from build cache download and install it - ps: >- if (-not (Test-Path C:\PX4)) { - Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.4.msi -OutFile C:\Toolchain.msi + Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.5.msi -OutFile C:\Toolchain.msi Start-Process -Wait msiexec -ArgumentList '/I C:\Toolchain.msi /quiet /qn /norestart /log C:\install.log' } @@ -30,8 +30,10 @@ install: # because otherwise the install begins but non-blocking and the result cannot be used just after build_script: +# FIXME Temporary we need to create the home folder because it's not contained in installer 0.5 and CI fails if it doesn't exist +- mkdir C:\PX4\home # setup the environmental variables to work within the installed cygwin toolchain -- call C:\PX4\toolchain\setup-environment-variables.bat x +- call C:\PX4\toolchain\scripts\setup-environment.bat x # safe the repopath for switching to it in cygwin bash - for /f %%i in ('cygpath -u %%CD%%') do set repopath=%%i # fetch all submodules in parallel From c1f851a60066ce9677d88dae72caa1e7becf5ff0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 25 Nov 2018 21:10:42 +0000 Subject: [PATCH 0109/1046] appveyor: shallow clone the main repo --- appveyor.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/appveyor.yml b/appveyor.yml index 8ff7a866eac3..c9a6725e5eba 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,7 @@ # Build version version: "{build}" +# shallow clone the main repo +clone_depth: 1 branches: only: From f692ad04d0ea22b8874926e9dc0cac23263bbeae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 16 Nov 2018 12:52:37 -0500 Subject: [PATCH 0110/1046] boards organization --- .ci/Jenkinsfile-SITL_tests | 26 +- .ci/Jenkinsfile-compile | 28 +- .ci/Jenkinsfile-hardware | 12 +- .gitmodules | 4 +- .travis.yml | 2 +- CMakeLists.txt | 188 +- Jenkinsfile | 34 +- Makefile | 61 +- ROMFS/CMakeLists.txt | 10 +- ROMFS/px4fmu_common/init.d/rcS | 4 +- Tools/astyle/files_to_check_code_style.sh | 1 - Tools/docker_run.sh | 4 +- Tools/sitl_multiple_run.sh | 4 +- .../aerotenna/ocpoc/src}/CMakeLists.txt | 4 +- .../aerotenna/ocpoc/src}/board_config.h | 2 +- boards/aerotenna/ocpoc/ubuntu.cmake | 86 + boards/airmind/mindpx-v2/default.cmake | 116 ++ .../airmind/mindpx-v2/firmware.prototype | 0 .../mindpx-v2/nuttx-config}/include/board.h | 0 .../mindpx-v2/nuttx-config}/nsh/defconfig | 2 +- .../mindpx-v2/nuttx-config/scripts/ld.script | 2 +- .../airmind/mindpx-v2/src}/CMakeLists.txt | 0 .../airmind/mindpx-v2/src}/board_config.h | 2 +- .../airmind/mindpx-v2/src}/can.c | 0 .../airmind/mindpx-v2/src}/init.c | 0 .../airmind/mindpx-v2/src}/led.c | 0 .../airmind/mindpx-v2/src}/spi.c | 0 .../airmind/mindpx-v2/src}/timer_config.c | 0 .../airmind/mindpx-v2/src}/usb.c | 0 {cmake => boards/atlflight}/cmake_hexagon | 0 boards/atlflight/eagle/default.cmake | 115 ++ boards/atlflight/eagle/qurt-default.cmake | 83 + .../atlflight/eagle/src}/CMakeLists.txt | 4 +- .../atlflight/eagle/src}/board_config.h | 2 +- .../atlflight/eagle/src}/sitl_led.c | 0 boards/atlflight/excelsior/default.cmake | 115 ++ boards/atlflight/excelsior/qurt-default.cmake | 82 + .../atlflight/excelsior/src}/board_config.h | 0 boards/atmel/same70xplained/default.cmake | 116 ++ .../atmel/same70xplained/firmware.prototype | 2 +- .../nuttx-config}/include/board.h | 0 .../nuttx-config}/nsh/defconfig | 4 +- .../nuttx-config}/scripts/ld.script | 0 .../atmel/same70xplained/src}/CMakeLists.txt | 0 .../atmel/same70xplained/src}/README.md | 4 +- .../atmel/same70xplained/src}/board_config.h | 2 +- .../atmel/same70xplained/src}/can.c | 0 .../atmel/same70xplained/src}/init.c | 0 .../atmel/same70xplained/src}/led.c | 0 .../atmel/same70xplained/src}/sdram.c | 0 .../atmel/same70xplained/src}/spi.c | 0 .../atmel/same70xplained/src}/timer_config.c | 0 .../atmel/same70xplained/src}/usb.c | 0 .../auav/esc35-v1/default.cmake | 64 +- .../auav/esc35-v1/firmware.prototype | 0 .../auav/esc35-v1/nuttx-config}/Kconfig | 0 .../esc35-v1/nuttx-config}/include/README.txt | 0 .../esc35-v1/nuttx-config}/include/board.h | 0 .../auav/esc35-v1/nuttx-config}/nsh/defconfig | 4 +- .../esc35-v1/nuttx-config}/scripts/ld.script | 0 .../auav/esc35-v1/src}/CMakeLists.txt | 0 .../auav/esc35-v1/src}/board_config.h | 2 +- .../auav/esc35-v1/src}/init.c | 0 .../auav/esc35-v1/src}/led.c | 0 .../auav/esc35-v1/src}/usb.c | 0 boards/auav/x21/default.cmake | 122 ++ .../auav/x21/firmware.prototype | 0 .../auav/x21/nuttx-config}/include/board.h | 0 .../auav/x21/nuttx-config}/nsh/defconfig | 2 +- .../auav/x21/nuttx-config}/scripts/ld.script | 16 +- .../auav/x21/src}/CMakeLists.txt | 0 .../auav/x21/src}/board_config.h | 2 +- .../auav-x21 => boards/auav/x21/src}/can.c | 0 .../auav-x21 => boards/auav/x21/src}/init.c | 0 .../auav-x21 => boards/auav/x21/src}/led.c | 0 .../auav-x21 => boards/auav/x21/src}/spi.c | 0 .../auav/x21/src}/timer_config.c | 2 +- .../auav-x21 => boards/auav/x21/src}/usb.c | 2 +- boards/av/x-v1/default.cmake | 121 ++ .../av/x-v1/firmware.prototype | 0 .../av/x-v1/nuttx-config}/include/board.h | 0 .../av/x-v1/nuttx-config}/nsh/defconfig | 4 +- .../av/x-v1/nuttx-config}/scripts/ld.script | 2 +- .../av/x-v1/src}/CMakeLists.txt | 0 .../av/x-v1/src}/board_config.h | 2 +- .../av-x-v1 => boards/av/x-v1/src}/init.c | 0 .../av-x-v1 => boards/av/x-v1/src}/manifest.c | 0 .../av-x-v1 => boards/av/x-v1/src}/sdio.c | 0 .../av-x-v1 => boards/av/x-v1/src}/spi.c | 0 .../av/x-v1/src}/timer_config.c | 0 boards/beaglebone/blue/cross.cmake | 84 + boards/beaglebone/blue/native.cmake | 83 + .../beaglebone/blue/src}/CMakeLists.txt | 2 +- .../beaglebone/blue/src}/board_config.h | 2 +- .../beaglebone/blue/src}/init.c | 0 boards/bitcraze/crazyflie/default.cmake | 73 + .../bitcraze/crazyflie/firmware.prototype | 0 .../crazyflie/nuttx-config}/include/board.h | 0 .../crazyflie/nuttx-config}/nsh/defconfig | 2 +- .../crazyflie/nuttx-config}/scripts/ld.script | 0 .../bitcraze/crazyflie/src}/CMakeLists.txt | 0 .../bitcraze/crazyflie/src}/board_config.h | 2 +- .../bitcraze/crazyflie/src}/init.c | 0 .../bitcraze/crazyflie/src}/led.c | 0 .../bitcraze/crazyflie/src}/spi.c | 0 .../bitcraze/crazyflie/src}/timer_config.c | 0 .../bitcraze/crazyflie/src}/usb.c | 0 boards/emlid/navio2/cross.cmake | 94 + boards/emlid/navio2/native.cmake | 93 + .../emlid/navio2/src}/CMakeLists.txt | 3 +- .../emlid/navio2/src}/board_config.h | 6 +- boards/gumstix/aerocore2/default.cmake | 119 ++ .../gumstix/aerocore2/firmware.prototype | 0 .../gumstix/aerocore2/nuttx-config}/Kconfig | 0 .../aerocore2/nuttx-config}/include/board.h | 0 .../aerocore2/nuttx-config}/nsh/defconfig | 2 +- .../aerocore2/nuttx-config}/scripts/ld.script | 0 .../gumstix/aerocore2/src}/CMakeLists.txt | 0 .../gumstix/aerocore2/src}/board_config.h | 2 +- .../gumstix/aerocore2/src}/can.c | 0 .../gumstix/aerocore2/src}/init.c | 0 .../gumstix/aerocore2/src}/led.c | 0 .../gumstix/aerocore2/src}/spi.c | 0 .../gumstix/aerocore2/src}/timer_config.c | 0 .../gumstix/aerocore2/src}/usb.c | 0 boards/intel/aerofc-v1/default.cmake | 101 + .../intel/aerofc-v1/firmware.prototype | 0 .../intel/aerofc-v1/nuttx-config}/Kconfig | 0 .../aerofc-v1/nuttx-config}/include/board.h | 0 .../aerofc-v1/nuttx-config}/nsh/defconfig | 2 +- .../aerofc-v1/nuttx-config}/scripts/ld.script | 0 boards/intel/aerofc-v1/rtps.cmake | 104 + .../intel/aerofc-v1/src}/CMakeLists.txt | 0 .../intel/aerofc-v1/src}/board_config.h | 2 +- .../intel/aerofc-v1/src}/init.c | 0 .../intel/aerofc-v1/src}/led.c | 0 .../intel/aerofc-v1/src}/spi.c | 0 .../intel/aerofc-v1/src}/timer_config.c | 0 boards/nxp/hlite-v3/default.cmake | 118 ++ .../nxp/hlite-v3/firmware.prototype | 0 .../nxp/hlite-v3/nuttx-config}/Kconfig | 0 .../hlite-v3/nuttx-config}/include/board.h | 0 .../nxp/hlite-v3/nuttx-config}/nsh/defconfig | 0 .../hlite-v3/nuttx-config}/scripts/ld.script | 0 .../nxp/hlite-v3/src}/CMakeLists.txt | 0 .../nxp/hlite-v3/src}/autoleds.c | 0 .../nxp/hlite-v3/src}/automount.c | 0 .../nxp/hlite-v3/src}/board_config.h | 2 +- .../nxp/hlite-v3/src}/can.c | 0 .../nxp/hlite-v3/src}/init.c | 0 .../nxp/hlite-v3/src}/led.c | 0 .../nxp/hlite-v3/src}/sdhc.c | 0 .../nxp/hlite-v3/src}/spi.c | 0 .../nxp/hlite-v3/src}/timer_config.c | 0 .../nxp/hlite-v3/src}/usb.c | 0 boards/omnibus/f4sd/default.cmake | 113 ++ .../omnibus/f4sd/firmware.prototype | 0 .../f4sd/nuttx-config}/include/board.h | 0 .../omnibus/f4sd/nuttx-config}/nsh/defconfig | 4 +- .../f4sd/nuttx-config}/scripts/ld.script | 0 .../omnibus/f4sd/src}/CMakeLists.txt | 0 .../omnibus/f4sd/src}/board_config.h | 2 +- .../omnibus/f4sd/src}/init.c | 0 .../omnibus/f4sd/src}/led.c | 0 .../omnibus/f4sd/src}/spi.c | 0 .../omnibus/f4sd/src}/timer_config.c | 0 .../omnibus/f4sd/src}/usb.c | 0 boards/parrot/bebop/default.cmake | 64 + .../parrot/bebop/src}/CMakeLists.txt | 3 +- .../parrot/bebop/src}/board_config.h | 2 +- .../px4/cannode-v1/default.cmake | 50 +- .../px4/cannode-v1/firmware.prototype | 0 .../px4/cannode-v1/nuttx-config}/Kconfig | 0 .../nuttx-config}/include/README.txt | 0 .../cannode-v1/nuttx-config}/include/board.h | 0 .../cannode-v1/nuttx-config}/nsh/defconfig | 0 .../nuttx-config}/nsh/defconfig.nonsh | 0 .../nuttx-config}/nsh/defconfig.nsh | 0 .../nuttx-config}/scripts/ld.script | 0 .../px4/cannode-v1/src}/CMakeLists.txt | 0 .../px4/cannode-v1/src}/board_config.h | 2 +- .../px4/cannode-v1/src}/buttons.c | 0 .../px4/cannode-v1/src}/can.c | 0 .../px4/cannode-v1/src}/init.c | 0 .../px4/cannode-v1/src}/led.c | 0 .../px4/cannode-v1/src}/spi.c | 0 .../px4/esc-v1/default.cmake | 58 +- .../px4/esc-v1/firmware.prototype | 0 .../px4/esc-v1/nuttx-config}/Kconfig | 0 .../esc-v1/nuttx-config}/include/README.txt | 0 .../px4/esc-v1/nuttx-config}/include/board.h | 0 .../px4/esc-v1/nuttx-config}/nsh/defconfig | 0 .../esc-v1/nuttx-config}/scripts/ld.script | 0 .../px4/esc-v1/src}/CMakeLists.txt | 0 .../px4/esc-v1/src}/board_config.h | 2 +- .../px4/esc-v1/src}/init.c | 0 .../px4esc-v1 => boards/px4/esc-v1/src}/led.c | 0 .../px4esc-v1 => boards/px4/esc-v1/src}/usb.c | 0 boards/px4/fmu-v2/default.cmake | 121 ++ .../px4/fmu-v2/firmware.prototype | 10 +- boards/px4/fmu-v2/lpe.cmake | 118 ++ .../px4/fmu-v2/nuttx-config}/Kconfig | 0 .../px4/fmu-v2/nuttx-config}/include/board.h | 0 .../px4/fmu-v2/nuttx-config}/nsh/defconfig | 0 .../fmu-v2/nuttx-config}/scripts/ld.script | 0 .../px4/fmu-v2/src}/CMakeLists.txt | 0 .../px4/fmu-v2/src}/board_config.h | 2 +- .../px4fmu-v2 => boards/px4/fmu-v2/src}/can.c | 0 .../px4fmu-v2 => boards/px4/fmu-v2/src}/i2c.c | 0 .../px4/fmu-v2/src}/init.c | 0 .../px4fmu-v2 => boards/px4/fmu-v2/src}/led.c | 0 .../px4/fmu-v2/src}/manifest.c | 0 .../px4fmu-v2 => boards/px4/fmu-v2/src}/spi.c | 0 .../px4/fmu-v2/src}/timer_config.c | 0 .../px4fmu-v2 => boards/px4/fmu-v2/src}/usb.c | 0 boards/px4/fmu-v2/test.cmake | 119 ++ boards/px4/fmu-v3/default.cmake | 129 ++ .../px4/fmu-v3/firmware.prototype | 0 .../px4/fmu-v3/nuttx-config}/Kconfig | 2 +- .../px4/fmu-v3/nuttx-config}/include/board.h | 222 ++- boards/px4/fmu-v3/nuttx-config/nsh/defconfig | 1688 +++++++++++++++++ .../fmu-v3/nuttx-config}/scripts/ld.script | 18 +- .../fmu-v3/nuttx-config}/stackcheck/defconfig | 4 +- boards/px4/fmu-v3/rtps.cmake | 130 ++ .../px4/fmu-v3/src}/CMakeLists.txt | 17 +- boards/px4/fmu-v3/src/board_config.h | 569 ++++++ .../px4fmu-v4 => boards/px4/fmu-v3/src}/can.c | 0 .../px4/fmu-v3/src/i2c.c | 25 +- boards/px4/fmu-v3/src/init.c | 570 ++++++ boards/px4/fmu-v3/src/led.c | 96 + boards/px4/fmu-v3/src/manifest.c | 149 ++ boards/px4/fmu-v3/src/spi.c | 462 +++++ boards/px4/fmu-v3/src/timer_config.c | 126 ++ .../px4/fmu-v3/src}/usb.c | 0 boards/px4/fmu-v3/stackcheck.cmake | 129 ++ boards/px4/fmu-v4/default.cmake | 114 ++ .../px4/fmu-v4/firmware.prototype | 0 .../px4/fmu-v4/nuttx-config}/include/board.h | 0 .../px4/fmu-v4/nuttx-config}/nsh/defconfig | 0 .../fmu-v4/nuttx-config}/scripts/ld.script | 24 +- .../fmu-v4/nuttx-config}/stackcheck/defconfig | 0 boards/px4/fmu-v4/rtps.cmake | 116 ++ .../px4/fmu-v4/src}/CMakeLists.txt | 0 .../px4/fmu-v4/src}/board_config.h | 2 +- .../px4/fmu-v4/src}/can.c | 0 .../px4/fmu-v4/src}/init.c | 0 .../px4fmu-v4 => boards/px4/fmu-v4/src}/led.c | 0 .../px4fmu-v4 => boards/px4/fmu-v4/src}/spi.c | 0 .../px4/fmu-v4/src}/timer_config.c | 0 .../px4fmu-v4 => boards/px4/fmu-v4/src}/usb.c | 0 boards/px4/fmu-v4/stackcheck.cmake | 114 ++ boards/px4/fmu-v4pro/default.cmake | 128 ++ .../px4/fmu-v4pro/firmware.prototype | 0 .../px4/fmu-v4pro/nuttx-config}/Kconfig | 0 .../fmu-v4pro/nuttx-config}/include/board.h | 0 .../px4/fmu-v4pro/nuttx-config}/nsh/defconfig | 0 .../fmu-v4pro/nuttx-config}/scripts/ld.script | 16 +- boards/px4/fmu-v4pro/rtps.cmake | 129 ++ .../px4/fmu-v4pro/src}/CMakeLists.txt | 0 .../px4/fmu-v4pro/src}/board_config.h | 2 +- .../px4/fmu-v4pro/src}/can.c | 11 +- .../px4/fmu-v4pro/src}/init.c | 0 .../px4/fmu-v4pro/src}/led.c | 0 .../px4/fmu-v4pro/src}/spi.c | 0 .../px4/fmu-v4pro/src}/timer_config.c | 0 boards/px4/fmu-v4pro/src/usb.c | 107 ++ boards/px4/fmu-v5/default.cmake | 129 ++ .../px4/fmu-v5/firmware.prototype | 0 .../px4/fmu-v5/nuttx-config}/Kconfig | 0 .../px4/fmu-v5/nuttx-config}/include/board.h | 0 .../px4/fmu-v5/nuttx-config}/nsh/defconfig | 0 .../fmu-v5/nuttx-config}/scripts/ld.script | 2 +- .../fmu-v5/nuttx-config}/stackcheck/defconfig | 0 boards/px4/fmu-v5/rtps.cmake | 127 ++ .../px4/fmu-v5/src}/CMakeLists.txt | 0 .../px4/fmu-v5/src}/board_config.h | 2 +- .../px4fmu-v5 => boards/px4/fmu-v5/src}/can.c | 0 .../px4/fmu-v5/src}/init.c | 0 .../px4fmu-v5 => boards/px4/fmu-v5/src}/led.c | 0 .../px4/fmu-v5/src}/manifest.c | 0 .../px4/fmu-v5/src}/sdio.c | 0 .../px4fmu-v5 => boards/px4/fmu-v5/src}/spi.c | 0 .../px4/fmu-v5/src}/timer_config.c | 0 .../px4fmu-v5 => boards/px4/fmu-v5/src}/usb.c | 0 boards/px4/fmu-v5/stackcheck.cmake | 126 ++ boards/px4/io-v2/default.cmake | 11 + .../px4/io-v2/firmware.prototype | 0 .../px4/io-v2/nuttx-config}/include/board.h | 0 .../px4/io-v2/nuttx-config}/nsh/defconfig | 0 .../px4/io-v2/nuttx-config}/scripts/ld.script | 0 .../px4/io-v2/src}/CMakeLists.txt | 0 .../px4/io-v2/src}/board_config.h | 2 +- .../px4io-v2 => boards/px4/io-v2/src}/init.c | 0 .../px4/io-v2/src}/timer_config.c | 0 boards/px4/raspberrypi/cross.cmake | 81 + boards/px4/raspberrypi/native.cmake | 80 + .../px4/raspberrypi/src}/CMakeLists.txt | 3 +- boards/px4/raspberrypi/src/board_config.h | 57 + boards/px4/sitl/default.cmake | 96 + boards/px4/sitl/rtps.cmake | 98 + .../px4/sitl/src}/CMakeLists.txt | 4 +- .../px4/sitl/src}/board_config.h | 2 +- .../eagle => boards/px4/sitl/src}/sitl_led.c | 0 boards/px4/sitl/test.cmake | 96 + boards/stm/32f4discovery/default.cmake | 106 ++ .../stm/32f4discovery/firmware.prototype | 2 +- .../nuttx-config}/include/board.h | 0 .../32f4discovery/nuttx-config}/nsh/defconfig | 4 +- .../nuttx-config}/scripts/ld.script | 0 .../stm/32f4discovery/src}/CMakeLists.txt | 1 + .../stm/32f4discovery/src}/board_config.h | 2 +- .../stm/32f4discovery/src}/init.c | 0 .../stm/32f4discovery/src}/led.c | 0 boards/stm/32f4discovery/src/timer_config.c | 76 + .../stm/32f4discovery/src}/usb.c | 0 boards/stm/nucleo-F767ZI/default.cmake | 115 ++ .../stm/nucleo-F767ZI/firmware.prototype | 4 +- .../nuttx-config}/include/board.h | 2 +- .../nucleo-F767ZI/nuttx-config}/nsh/defconfig | 4 +- .../nuttx-config}/scripts/ld.script | 0 .../stm/nucleo-F767ZI/src}/CMakeLists.txt | 0 .../stm/nucleo-F767ZI/src}/board_config.h | 2 +- .../stm/nucleo-F767ZI/src}/can.c | 0 .../stm/nucleo-F767ZI/src}/init.c | 0 .../stm/nucleo-F767ZI/src}/led.c | 0 .../stm/nucleo-F767ZI/src}/sdio.c | 0 .../stm/nucleo-F767ZI/src}/spi.c | 0 .../stm/nucleo-F767ZI/src}/timer_config.c | 0 .../stm/nucleo-F767ZI/src}/usb.c | 0 .../thiemar/s2740vc-v1/default.cmake | 44 +- .../thiemar/s2740vc-v1/firmware.prototype | 0 .../thiemar/s2740vc-v1/nuttx-config}/Kconfig | 0 .../nuttx-config}/include/README.txt | 0 .../s2740vc-v1/nuttx-config}/include/board.h | 0 .../nuttx-config}/nsh/IMPORTANT_README.txt | 0 .../s2740vc-v1/nuttx-config}/nsh/defconfig | 2 +- .../nuttx-config}/scripts/ld.script | 0 .../thiemar/s2740vc-v1/src}/CMakeLists.txt | 0 .../thiemar/s2740vc-v1/src}/board_config.h | 2 +- .../thiemar/s2740vc-v1/src}/can.c | 0 .../thiemar/s2740vc-v1/src}/init.c | 0 cmake/configs/nuttx_aerocore2_default.cmake | 135 -- cmake/configs/nuttx_aerofc-v1_default.cmake | 75 - cmake/configs/nuttx_aerofc-v1_rtps.cmake | 7 - cmake/configs/nuttx_auav-x21_default.cmake | 158 -- cmake/configs/nuttx_av-x-v1_default.cmake | 154 -- cmake/configs/nuttx_crazyflie_default.cmake | 104 - cmake/configs/nuttx_mindpx-v2_default.cmake | 150 -- cmake/configs/nuttx_nxphlite-v3_default.cmake | 163 -- .../configs/nuttx_omnibus-f4sd_default.cmake | 143 -- .../nuttx_px4-same70xplained-v1_default.cmake | 122 -- .../nuttx_px4-stm32f4discovery_default.cmake | 41 - cmake/configs/nuttx_px4fmu-v2_default.cmake | 187 -- cmake/configs/nuttx_px4fmu-v2_lpe.cmake | 16 - cmake/configs/nuttx_px4fmu-v2_test.cmake | 163 -- cmake/configs/nuttx_px4fmu-v3_default.cmake | 170 -- cmake/configs/nuttx_px4fmu-v3_rtps.cmake | 7 - .../configs/nuttx_px4fmu-v3_stackcheck.cmake | 128 -- cmake/configs/nuttx_px4fmu-v4_default.cmake | 158 -- cmake/configs/nuttx_px4fmu-v4_rtps.cmake | 5 - .../configs/nuttx_px4fmu-v4_stackcheck.cmake | 116 -- .../configs/nuttx_px4fmu-v4pro_default.cmake | 162 -- cmake/configs/nuttx_px4fmu-v4pro_rtps.cmake | 5 - cmake/configs/nuttx_px4fmu-v5_default.cmake | 164 -- cmake/configs/nuttx_px4fmu-v5_rtps.cmake | 5 - .../configs/nuttx_px4fmu-v5_stackcheck.cmake | 121 -- cmake/configs/nuttx_px4io-v2_default.cmake | 7 - .../nuttx_px4nucleoF767ZI-v1_default.cmake | 128 -- cmake/configs/posix_bbblue_common.cmake | 104 - cmake/configs/posix_bbblue_cross.cmake | 3 - cmake/configs/posix_bbblue_native.cmake | 7 - cmake/configs/posix_bebop_default.cmake | 75 - cmake/configs/posix_eagle_default.cmake | 17 - cmake/configs/posix_eagle_hil.cmake | 38 - cmake/configs/posix_eagle_legacy.cmake | 15 - cmake/configs/posix_eagle_muorb.cmake | 19 - cmake/configs/posix_excelsior_default.cmake | 11 - cmake/configs/posix_excelsior_legacy.cmake | 14 - cmake/configs/posix_ocpoc_cross.cmake | 85 - cmake/configs/posix_ocpoc_ubuntu.cmake | 84 - cmake/configs/posix_rpi_common.cmake | 103 - cmake/configs/posix_rpi_cross.cmake | 3 - cmake/configs/posix_rpi_cross_no_shield.cmake | 19 - cmake/configs/posix_rpi_native.cmake | 7 - cmake/configs/posix_sdflight_default.cmake | 66 - cmake/configs/posix_sdflight_legacy.cmake | 56 - cmake/configs/posix_sdflight_rtps.cmake | 5 - cmake/configs/posix_sitl_broadcast.cmake | 3 - cmake/configs/posix_sitl_default.cmake | 164 -- cmake/configs/posix_sitl_rtps.cmake | 5 - cmake/configs/posix_sitl_test.cmake | 2 - cmake/configs/qurt_eagle_default.cmake | 12 - cmake/configs/qurt_eagle_hello.cmake | 33 - cmake/configs/qurt_eagle_hil.cmake | 59 - cmake/configs/qurt_eagle_legacy.cmake | 12 - cmake/configs/qurt_eagle_muorb.cmake | 38 - cmake/configs/qurt_eagle_test.cmake | 33 - cmake/configs/qurt_eagle_travis.cmake | 65 - cmake/configs/qurt_excelsior_default.cmake | 12 - cmake/configs/qurt_excelsior_legacy.cmake | 14 - cmake/configs/qurt_sdflight_default.cmake | 93 - cmake/configs/qurt_sdflight_legacy.cmake | 88 - cmake/{common => }/coverage.cmake | 0 cmake/px4_add_board.cmake | 200 ++ cmake/{common => }/px4_base.cmake | 95 +- cmake/{common => }/px4_git.cmake | 2 +- cmake/px4_make_uavcan_bootloader.cmake | 99 + cmake/{common => }/px4_metadata.cmake | 2 +- cmake/{common => }/sanitizers.cmake | 0 .../Toolchain-arm-linux-gnueabihf.cmake | 30 - .../toolchains/Toolchain-arm-none-eabi.cmake | 82 - .../Toolchain-arm-xilinx-linux-gnueabi.cmake | 88 - .../Toolchain-gcc-arm-linux-gnueabihf.cmake | 74 - eclipse.cproject | 16 +- eclipse.project | 4 +- platforms/nuttx/CMakeLists.txt | 30 +- platforms/nuttx/Debug/poor-mans-profiler.sh | 2 +- platforms/nuttx/Images/px4flow-v2.prototype | 13 - platforms/nuttx/Images/zubaxgnss-v1.prototype | 13 - platforms/nuttx/NuttX/CMakeLists.txt | 84 +- .../{nuttx-configs => NuttX}/Make.defs.in | 53 +- .../include => NuttX}/nsh_romfsimg.h | 0 .../nuttx/NuttX/tools/new_nuttx_px_config.sh | 173 -- .../Generic-arm-none-eabi-gcc-cortex-m3.cmake | 6 + .../Generic-arm-none-eabi-gcc-cortex-m4.cmake | 6 + .../Generic-arm-none-eabi-gcc-cortex-m7.cmake | 6 + .../nuttx/cmake/Toolchain-arm-none-eabi.cmake | 48 + platforms/nuttx/cmake/px4_add_board_os.cmake | 130 ++ platforms/nuttx/cmake/px4_impl_os.cmake | 160 +- .../auav-x21/include/nsh_romfsimg.h | 42 - .../av-x-v1/include/nsh_romfsimg.h | 42 - .../crazyflie/include/nsh_romfsimg.h | 42 - .../mindpx-v2/include/nsh_romfsimg.h | 42 - .../nxphlite-v3/include/nsh_romfsimg.h | 42 - .../omnibus-f4sd/include/nsh_romfsimg.h | 42 - .../px4-same70xplained-v1/Kconfig | 22 - .../include/nsh_romfsimg.h | 42 - .../px4-same70xplained-v1/nsh/setenv.sh | 80 - .../scripts/flash-dtcm.ld | 123 -- .../scripts/flash-sram.ld | 120 -- .../px4-same70xplained-v1/scripts/gnu-elf.ld | 129 -- .../scripts/kernel-space.ld | 111 -- .../px4-same70xplained-v1/scripts/memory.ld | 84 - .../scripts/user-space.ld | 126 -- .../include/nsh_romfsimg.h | 42 - .../nuttx/nuttx-configs/px4flow-v2/Kconfig | 22 - .../px4flow-v2/include/README.txt | 2 - .../nuttx-configs/px4flow-v2/nsh/defconfig | 1004 ---------- .../px4flow-v2/scripts/ld.script | 158 -- .../px4fmu-v2/include/nsh_romfsimg.h | 42 - .../px4fmu-v4/include/nsh_romfsimg.h | 42 - .../px4fmu-v4pro/include/nsh_romfsimg.h | 42 - .../px4fmu-v5/include/nsh_romfsimg.h | 42 - .../px4nucleoF767ZI-v1/include/nsh_romfsimg.h | 42 - .../nuttx/nuttx-configs/zubaxgnss-v1/Kconfig | 22 - .../zubaxgnss-v1/include/README.txt | 4 - .../zubaxgnss-v1/include/board.h | 251 --- .../nuttx-configs/zubaxgnss-v1/nsh/defconfig | 1070 ----------- .../zubaxgnss-v1/scripts/ld.script | 153 -- platforms/nuttx/src/px4_layer/CMakeLists.txt | 2 +- platforms/posix/CMakeLists.txt | 26 +- ...Linux-arm-linux-gnueabihf-cortex-a53.cmake | 6 + .../Linux-arm-linux-gnueabihf-cortex-a8.cmake | 6 + .../Linux-arm-linux-gnueabihf-cortex-a9.cmake | 6 + .../cmake/Toolchain-arm-linux-gnueabihf.cmake | 45 + .../posix/cmake}/Toolchain-native.cmake | 0 platforms/posix/cmake/px4_add_board_os.cmake | 106 ++ platforms/posix/cmake/px4_impl_os.cmake | 73 +- platforms/posix/src/px4_layer/CMakeLists.txt | 2 +- platforms/qurt/CMakeLists.txt | 3 +- platforms/qurt/cmake/px4_add_board_os.cmake | 101 + platforms/qurt/cmake/px4_impl_os.cmake | 15 +- platforms/qurt/include/board_config.h | 2 +- platforms/qurt/src/px4_layer/CMakeLists.txt | 2 +- src/drivers/bbblue_adc/CMakeLists.txt | 2 +- src/drivers/boards/CMakeLists.txt | 9 +- src/drivers/boards/common/CMakeLists.txt | 2 +- src/drivers/boards/common/board_common.h | 2 +- .../boards/common/stm32/CMakeLists.txt | 15 +- src/drivers/boards/px4flow-v2/board_config.h | 385 ---- src/drivers/boards/px4flow-v2/init.c | 190 -- src/drivers/boards/px4flow-v2/led.c | 192 -- .../boards/zubaxgnss-v1/board_config.h | 247 --- src/drivers/boards/zubaxgnss-v1/gnss_init.c | 186 -- src/drivers/boards/zubaxgnss-v1/gnss_led.c | 191 -- .../distance_sensor/sf0x/CMakeLists.txt | 5 +- src/drivers/px4io/CMakeLists.txt | 28 +- src/lib/cdev/CMakeLists.txt | 2 +- src/lib/controllib/CMakeLists.txt | 6 +- src/lib/drivers/device/CMakeLists.txt | 4 +- src/lib/parameters/CMakeLists.txt | 22 +- src/lib/rc/CMakeLists.txt | 4 + src/lib/rc/rc_tests/RCTest.cpp | 1 + src/modules/commander/CMakeLists.txt | 9 +- .../failure_detector}/CMakeLists.txt | 8 +- src/modules/mavlink/CMakeLists.txt | 4 + src/modules/micrortps_bridge/CMakeLists.txt | 2 +- src/modules/simulator/CMakeLists.txt | 2 +- src/modules/systemlib/CMakeLists.txt | 4 +- src/modules/uORB/CMakeLists.txt | 4 + src/modules/uavcan/CMakeLists.txt | 17 +- src/modules/uavcanesc/CMakeLists.txt | 2 +- src/modules/uavcannode/CMakeLists.txt | 2 +- src/platforms/common/CMakeLists.txt | 2 +- .../common/work_queue/CMakeLists.txt | 2 +- src/systemcmds/tests/CMakeLists.txt | 2 +- test/rostest_px4_run.sh | 2 +- 507 files changed, 9975 insertions(+), 10989 deletions(-) rename {src/drivers/boards/rpi => boards/aerotenna/ocpoc/src}/CMakeLists.txt (94%) rename {src/drivers/boards/ocpoc => boards/aerotenna/ocpoc/src}/board_config.h (98%) create mode 100644 boards/aerotenna/ocpoc/ubuntu.cmake create mode 100644 boards/airmind/mindpx-v2/default.cmake rename platforms/nuttx/Images/mindpx-v2.prototype => boards/airmind/mindpx-v2/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/mindpx-v2 => boards/airmind/mindpx-v2/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/mindpx-v2 => boards/airmind/mindpx-v2/nuttx-config}/nsh/defconfig (99%) rename platforms/nuttx/nuttx-configs/px4fmu-v2/scripts/ld_full.script => boards/airmind/mindpx-v2/nuttx-config/scripts/ld.script (99%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/CMakeLists.txt (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/board_config.h (99%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/can.c (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/init.c (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/led.c (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/spi.c (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/timer_config.c (100%) rename {src/drivers/boards/mindpx-v2 => boards/airmind/mindpx-v2/src}/usb.c (100%) rename {cmake => boards/atlflight}/cmake_hexagon (100%) create mode 100644 boards/atlflight/eagle/default.cmake create mode 100644 boards/atlflight/eagle/qurt-default.cmake rename {src/drivers/boards/sitl => boards/atlflight/eagle/src}/CMakeLists.txt (94%) rename {src/drivers/boards/eagle => boards/atlflight/eagle/src}/board_config.h (97%) rename {src/drivers/boards/sitl => boards/atlflight/eagle/src}/sitl_led.c (100%) create mode 100644 boards/atlflight/excelsior/default.cmake create mode 100644 boards/atlflight/excelsior/qurt-default.cmake rename {src/drivers/boards/excelsior => boards/atlflight/excelsior/src}/board_config.h (100%) create mode 100644 boards/atmel/same70xplained/default.cmake rename platforms/nuttx/Images/px4-same70xplained-v1.prototype => boards/atmel/same70xplained/firmware.prototype (87%) rename {platforms/nuttx/nuttx-configs/px4-same70xplained-v1 => boards/atmel/same70xplained/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/px4-same70xplained-v1 => boards/atmel/same70xplained/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/px4-same70xplained-v1 => boards/atmel/same70xplained/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/README.md (88%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/board_config.h (99%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/can.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/init.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/led.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/sdram.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/spi.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/timer_config.c (100%) rename {src/drivers/boards/px4-same70xplained-v1 => boards/atmel/same70xplained/src}/usb.c (100%) rename cmake/configs/nuttx_esc35-v1_default.cmake => boards/auav/esc35-v1/default.cmake (60%) rename platforms/nuttx/Images/esc35-v1.prototype => boards/auav/esc35-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/esc35-v1 => boards/auav/esc35-v1/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/esc35-v1 => boards/auav/esc35-v1/nuttx-config}/include/README.txt (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/esc35-v1 => boards/auav/esc35-v1/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/esc35-v1 => boards/auav/esc35-v1/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/esc35-v1 => boards/auav/esc35-v1/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/esc35-v1 => boards/auav/esc35-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/esc35-v1 => boards/auav/esc35-v1/src}/board_config.h (99%) rename {src/drivers/boards/esc35-v1 => boards/auav/esc35-v1/src}/init.c (100%) rename {src/drivers/boards/esc35-v1 => boards/auav/esc35-v1/src}/led.c (100%) rename {src/drivers/boards/esc35-v1 => boards/auav/esc35-v1/src}/usb.c (100%) create mode 100644 boards/auav/x21/default.cmake rename platforms/nuttx/Images/auav-x21.prototype => boards/auav/x21/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/auav-x21 => boards/auav/x21/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/auav-x21 => boards/auav/x21/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/auav-x21 => boards/auav/x21/nuttx-config}/scripts/ld.script (93%) rename {src/drivers/boards/aerocore2 => boards/auav/x21/src}/CMakeLists.txt (100%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/board_config.h (99%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/can.c (100%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/init.c (100%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/led.c (100%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/spi.c (100%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/timer_config.c (99%) rename {src/drivers/boards/auav-x21 => boards/auav/x21/src}/usb.c (99%) create mode 100644 boards/av/x-v1/default.cmake rename platforms/nuttx/Images/av-x-v1.prototype => boards/av/x-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/av-x-v1 => boards/av/x-v1/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/av-x-v1 => boards/av/x-v1/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/av-x-v1 => boards/av/x-v1/nuttx-config}/scripts/ld.script (99%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/board_config.h (99%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/init.c (100%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/manifest.c (100%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/sdio.c (100%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/spi.c (100%) rename {src/drivers/boards/av-x-v1 => boards/av/x-v1/src}/timer_config.c (100%) create mode 100644 boards/beaglebone/blue/cross.cmake create mode 100644 boards/beaglebone/blue/native.cmake rename {src/drivers/boards/bbblue => boards/beaglebone/blue/src}/CMakeLists.txt (94%) rename {src/drivers/boards/bbblue => boards/beaglebone/blue/src}/board_config.h (98%) rename {src/drivers/boards/bbblue => boards/beaglebone/blue/src}/init.c (100%) create mode 100644 boards/bitcraze/crazyflie/default.cmake rename platforms/nuttx/Images/crazyflie.prototype => boards/bitcraze/crazyflie/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/crazyflie => boards/bitcraze/crazyflie/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/crazyflie => boards/bitcraze/crazyflie/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/crazyflie => boards/bitcraze/crazyflie/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/CMakeLists.txt (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/board_config.h (99%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/init.c (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/led.c (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/spi.c (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/timer_config.c (100%) rename {src/drivers/boards/crazyflie => boards/bitcraze/crazyflie/src}/usb.c (100%) create mode 100644 boards/emlid/navio2/cross.cmake create mode 100644 boards/emlid/navio2/native.cmake rename {src/drivers/boards/ocpoc => boards/emlid/navio2/src}/CMakeLists.txt (94%) rename {src/drivers/boards/rpi => boards/emlid/navio2/src}/board_config.h (94%) create mode 100644 boards/gumstix/aerocore2/default.cmake rename platforms/nuttx/Images/aerocore2.prototype => boards/gumstix/aerocore2/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/aerocore2 => boards/gumstix/aerocore2/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/aerocore2 => boards/gumstix/aerocore2/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/aerocore2 => boards/gumstix/aerocore2/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/aerocore2 => boards/gumstix/aerocore2/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/auav-x21 => boards/gumstix/aerocore2/src}/CMakeLists.txt (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/board_config.h (99%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/can.c (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/init.c (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/led.c (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/spi.c (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/timer_config.c (100%) rename {src/drivers/boards/aerocore2 => boards/gumstix/aerocore2/src}/usb.c (100%) create mode 100644 boards/intel/aerofc-v1/default.cmake rename platforms/nuttx/Images/aerofc-v1.prototype => boards/intel/aerofc-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/aerofc-v1 => boards/intel/aerofc-v1/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/aerofc-v1 => boards/intel/aerofc-v1/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/aerofc-v1 => boards/intel/aerofc-v1/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/aerofc-v1 => boards/intel/aerofc-v1/nuttx-config}/scripts/ld.script (100%) create mode 100644 boards/intel/aerofc-v1/rtps.cmake rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/board_config.h (99%) rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/init.c (100%) rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/led.c (100%) rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/spi.c (100%) rename {src/drivers/boards/aerofc-v1 => boards/intel/aerofc-v1/src}/timer_config.c (100%) create mode 100644 boards/nxp/hlite-v3/default.cmake rename platforms/nuttx/Images/nxphlite-v3.prototype => boards/nxp/hlite-v3/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/nxphlite-v3 => boards/nxp/hlite-v3/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/nxphlite-v3 => boards/nxp/hlite-v3/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/nxphlite-v3 => boards/nxp/hlite-v3/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/nxphlite-v3 => boards/nxp/hlite-v3/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/CMakeLists.txt (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/autoleds.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/automount.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/board_config.h (99%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/can.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/init.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/led.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/sdhc.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/spi.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/timer_config.c (100%) rename {src/drivers/boards/nxphlite-v3 => boards/nxp/hlite-v3/src}/usb.c (100%) create mode 100644 boards/omnibus/f4sd/default.cmake rename platforms/nuttx/Images/omnibus-f4sd.prototype => boards/omnibus/f4sd/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/omnibus-f4sd => boards/omnibus/f4sd/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/omnibus-f4sd => boards/omnibus/f4sd/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/omnibus-f4sd => boards/omnibus/f4sd/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/CMakeLists.txt (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/board_config.h (99%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/init.c (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/led.c (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/spi.c (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/timer_config.c (100%) rename {src/drivers/boards/omnibus-f4sd => boards/omnibus/f4sd/src}/usb.c (100%) create mode 100644 boards/parrot/bebop/default.cmake rename {src/drivers/boards/sim => boards/parrot/bebop/src}/CMakeLists.txt (94%) rename {src/drivers/boards/bebop => boards/parrot/bebop/src}/board_config.h (97%) rename cmake/configs/nuttx_px4cannode-v1_default.cmake => boards/px4/cannode-v1/default.cmake (71%) rename platforms/nuttx/Images/px4cannode-v1.prototype => boards/px4/cannode-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/include/README.txt (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/nsh/defconfig.nonsh (100%) rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/nsh/defconfig.nsh (100%) rename {platforms/nuttx/nuttx-configs/px4cannode-v1 => boards/px4/cannode-v1/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/board_config.h (99%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/buttons.c (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/can.c (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/init.c (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/led.c (100%) rename {src/drivers/boards/px4cannode-v1 => boards/px4/cannode-v1/src}/spi.c (100%) rename cmake/configs/nuttx_px4esc-v1_default.cmake => boards/px4/esc-v1/default.cmake (62%) rename platforms/nuttx/Images/px4esc-v1.prototype => boards/px4/esc-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4esc-v1 => boards/px4/esc-v1/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/px4esc-v1 => boards/px4/esc-v1/nuttx-config}/include/README.txt (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4esc-v1 => boards/px4/esc-v1/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4esc-v1 => boards/px4/esc-v1/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/px4esc-v1 => boards/px4/esc-v1/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4esc-v1 => boards/px4/esc-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4esc-v1 => boards/px4/esc-v1/src}/board_config.h (99%) rename {src/drivers/boards/px4esc-v1 => boards/px4/esc-v1/src}/init.c (100%) rename {src/drivers/boards/px4esc-v1 => boards/px4/esc-v1/src}/led.c (100%) rename {src/drivers/boards/px4esc-v1 => boards/px4/esc-v1/src}/usb.c (100%) create mode 100644 boards/px4/fmu-v2/default.cmake rename platforms/nuttx/Images/px4fmu-v2.prototype => boards/px4/fmu-v2/firmware.prototype (52%) create mode 100644 boards/px4/fmu-v2/lpe.cmake rename {platforms/nuttx/nuttx-configs/px4fmu-v2 => boards/px4/fmu-v2/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v2 => boards/px4/fmu-v2/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4fmu-v2 => boards/px4/fmu-v2/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v2 => boards/px4/fmu-v2/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/board_config.h (99%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/can.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/i2c.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/init.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/led.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/manifest.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/spi.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/timer_config.c (100%) rename {src/drivers/boards/px4fmu-v2 => boards/px4/fmu-v2/src}/usb.c (100%) create mode 100644 boards/px4/fmu-v2/test.cmake create mode 100644 boards/px4/fmu-v3/default.cmake rename platforms/nuttx/Images/px4fmu-v3.prototype => boards/px4/fmu-v3/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4nucleoF767ZI-v1 => boards/px4/fmu-v3/nuttx-config}/Kconfig (94%) rename {platforms/nuttx/nuttx-configs/px4flow-v2 => boards/px4/fmu-v3/nuttx-config}/include/board.h (62%) mode change 100755 => 100644 create mode 100644 boards/px4/fmu-v3/nuttx-config/nsh/defconfig rename {platforms/nuttx/nuttx-configs/px4fmu-v4 => boards/px4/fmu-v3/nuttx-config}/scripts/ld.script (93%) rename {platforms/nuttx/nuttx-configs/px4fmu-v2 => boards/px4/fmu-v3/nuttx-config}/stackcheck/defconfig (99%) create mode 100644 boards/px4/fmu-v3/rtps.cmake rename {src/drivers/boards/zubaxgnss-v1 => boards/px4/fmu-v3/src}/CMakeLists.txt (88%) create mode 100644 boards/px4/fmu-v3/src/board_config.h rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v3/src}/can.c (100%) rename platforms/nuttx/nuttx-configs/aerofc-v1/include/nsh_romfsimg.h => boards/px4/fmu-v3/src/i2c.c (81%) create mode 100644 boards/px4/fmu-v3/src/init.c create mode 100644 boards/px4/fmu-v3/src/led.c create mode 100644 boards/px4/fmu-v3/src/manifest.c create mode 100644 boards/px4/fmu-v3/src/spi.c create mode 100644 boards/px4/fmu-v3/src/timer_config.c rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v3/src}/usb.c (100%) create mode 100644 boards/px4/fmu-v3/stackcheck.cmake create mode 100644 boards/px4/fmu-v4/default.cmake rename platforms/nuttx/Images/px4fmu-v4.prototype => boards/px4/fmu-v4/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4 => boards/px4/fmu-v4/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4fmu-v4 => boards/px4/fmu-v4/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/mindpx-v2 => boards/px4/fmu-v4/nuttx-config}/scripts/ld.script (92%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4 => boards/px4/fmu-v4/nuttx-config}/stackcheck/defconfig (100%) create mode 100644 boards/px4/fmu-v4/rtps.cmake rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/board_config.h (99%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4/src}/can.c (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/init.c (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/led.c (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/spi.c (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/timer_config.c (100%) rename {src/drivers/boards/px4fmu-v4 => boards/px4/fmu-v4/src}/usb.c (100%) create mode 100644 boards/px4/fmu-v4/stackcheck.cmake create mode 100644 boards/px4/fmu-v4pro/default.cmake rename platforms/nuttx/Images/px4fmu-v4pro.prototype => boards/px4/fmu-v4pro/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4pro => boards/px4/fmu-v4pro/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4pro => boards/px4/fmu-v4pro/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4pro => boards/px4/fmu-v4pro/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v4pro => boards/px4/fmu-v4pro/nuttx-config}/scripts/ld.script (93%) create mode 100644 boards/px4/fmu-v4pro/rtps.cmake rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/board_config.h (99%) rename {src/drivers/boards/s2740vc-v1 => boards/px4/fmu-v4pro/src}/can.c (94%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/init.c (100%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/led.c (100%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/spi.c (100%) rename {src/drivers/boards/px4fmu-v4pro => boards/px4/fmu-v4pro/src}/timer_config.c (100%) create mode 100644 boards/px4/fmu-v4pro/src/usb.c create mode 100644 boards/px4/fmu-v5/default.cmake rename platforms/nuttx/Images/px4fmu-v5.prototype => boards/px4/fmu-v5/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v5 => boards/px4/fmu-v5/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v5 => boards/px4/fmu-v5/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4fmu-v5 => boards/px4/fmu-v5/nuttx-config}/nsh/defconfig (100%) rename {platforms/nuttx/nuttx-configs/px4fmu-v5 => boards/px4/fmu-v5/nuttx-config}/scripts/ld.script (99%) rename {platforms/nuttx/nuttx-configs/px4fmu-v5 => boards/px4/fmu-v5/nuttx-config}/stackcheck/defconfig (100%) create mode 100644 boards/px4/fmu-v5/rtps.cmake rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/board_config.h (99%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/can.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/init.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/led.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/manifest.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/sdio.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/spi.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/timer_config.c (100%) rename {src/drivers/boards/px4fmu-v5 => boards/px4/fmu-v5/src}/usb.c (100%) create mode 100644 boards/px4/fmu-v5/stackcheck.cmake create mode 100644 boards/px4/io-v2/default.cmake rename platforms/nuttx/Images/px4io-v2.prototype => boards/px4/io-v2/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/px4io-v2 => boards/px4/io-v2/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4io-v2 => boards/px4/io-v2/nuttx-config}/nsh/defconfig (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4io-v2 => boards/px4/io-v2/nuttx-config}/scripts/ld.script (100%) mode change 100755 => 100644 rename {src/drivers/boards/px4io-v2 => boards/px4/io-v2/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4io-v2 => boards/px4/io-v2/src}/board_config.h (99%) rename {src/drivers/boards/px4io-v2 => boards/px4/io-v2/src}/init.c (100%) rename {src/drivers/boards/px4io-v2 => boards/px4/io-v2/src}/timer_config.c (100%) create mode 100644 boards/px4/raspberrypi/cross.cmake create mode 100644 boards/px4/raspberrypi/native.cmake rename {src/drivers/boards/bebop => boards/px4/raspberrypi/src}/CMakeLists.txt (94%) create mode 100644 boards/px4/raspberrypi/src/board_config.h create mode 100644 boards/px4/sitl/default.cmake create mode 100644 boards/px4/sitl/rtps.cmake rename {src/drivers/boards/eagle => boards/px4/sitl/src}/CMakeLists.txt (94%) rename {src/drivers/boards/sitl => boards/px4/sitl/src}/board_config.h (98%) rename {src/drivers/boards/eagle => boards/px4/sitl/src}/sitl_led.c (100%) create mode 100644 boards/px4/sitl/test.cmake create mode 100644 boards/stm/32f4discovery/default.cmake rename platforms/nuttx/Images/px4-stm32f4discovery.prototype => boards/stm/32f4discovery/firmware.prototype (87%) rename {platforms/nuttx/nuttx-configs/px4-stm32f4discovery => boards/stm/32f4discovery/nuttx-config}/include/board.h (100%) rename {platforms/nuttx/nuttx-configs/px4-stm32f4discovery => boards/stm/32f4discovery/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/px4-stm32f4discovery => boards/stm/32f4discovery/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4-stm32f4discovery => boards/stm/32f4discovery/src}/CMakeLists.txt (99%) rename {src/drivers/boards/px4-stm32f4discovery => boards/stm/32f4discovery/src}/board_config.h (99%) rename {src/drivers/boards/px4-stm32f4discovery => boards/stm/32f4discovery/src}/init.c (100%) rename {src/drivers/boards/px4-stm32f4discovery => boards/stm/32f4discovery/src}/led.c (100%) create mode 100644 boards/stm/32f4discovery/src/timer_config.c rename {src/drivers/boards/px4-stm32f4discovery => boards/stm/32f4discovery/src}/usb.c (100%) create mode 100644 boards/stm/nucleo-F767ZI/default.cmake rename platforms/nuttx/Images/px4nucleoF767ZI-v1.prototype => boards/stm/nucleo-F767ZI/firmware.prototype (64%) rename {platforms/nuttx/nuttx-configs/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/nuttx-config}/include/board.h (99%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/CMakeLists.txt (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/board_config.h (99%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/can.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/init.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/led.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/sdio.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/spi.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/timer_config.c (100%) rename {src/drivers/boards/px4nucleoF767ZI-v1 => boards/stm/nucleo-F767ZI/src}/usb.c (100%) rename cmake/configs/nuttx_s2740vc-v1_default.cmake => boards/thiemar/s2740vc-v1/default.cmake (68%) rename platforms/nuttx/Images/s2740vc-v1.prototype => boards/thiemar/s2740vc-v1/firmware.prototype (100%) rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/Kconfig (100%) rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/include/README.txt (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/include/board.h (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/nsh/IMPORTANT_README.txt (100%) mode change 100755 => 100644 rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/nsh/defconfig (99%) rename {platforms/nuttx/nuttx-configs/s2740vc-v1 => boards/thiemar/s2740vc-v1/nuttx-config}/scripts/ld.script (100%) rename {src/drivers/boards/s2740vc-v1 => boards/thiemar/s2740vc-v1/src}/CMakeLists.txt (100%) rename {src/drivers/boards/s2740vc-v1 => boards/thiemar/s2740vc-v1/src}/board_config.h (99%) rename {src/drivers/boards/px4flow-v2 => boards/thiemar/s2740vc-v1/src}/can.c (100%) rename {src/drivers/boards/s2740vc-v1 => boards/thiemar/s2740vc-v1/src}/init.c (100%) delete mode 100644 cmake/configs/nuttx_aerocore2_default.cmake delete mode 100644 cmake/configs/nuttx_aerofc-v1_default.cmake delete mode 100644 cmake/configs/nuttx_aerofc-v1_rtps.cmake delete mode 100644 cmake/configs/nuttx_auav-x21_default.cmake delete mode 100644 cmake/configs/nuttx_av-x-v1_default.cmake delete mode 100644 cmake/configs/nuttx_crazyflie_default.cmake delete mode 100644 cmake/configs/nuttx_mindpx-v2_default.cmake delete mode 100644 cmake/configs/nuttx_nxphlite-v3_default.cmake delete mode 100644 cmake/configs/nuttx_omnibus-f4sd_default.cmake delete mode 100644 cmake/configs/nuttx_px4-same70xplained-v1_default.cmake delete mode 100644 cmake/configs/nuttx_px4-stm32f4discovery_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v2_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v2_lpe.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v2_test.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v3_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v3_rtps.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v3_stackcheck.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v4_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v4_rtps.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v4_stackcheck.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v4pro_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v4pro_rtps.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v5_default.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v5_rtps.cmake delete mode 100644 cmake/configs/nuttx_px4fmu-v5_stackcheck.cmake delete mode 100644 cmake/configs/nuttx_px4io-v2_default.cmake delete mode 100644 cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake delete mode 100644 cmake/configs/posix_bbblue_common.cmake delete mode 100644 cmake/configs/posix_bbblue_cross.cmake delete mode 100644 cmake/configs/posix_bbblue_native.cmake delete mode 100644 cmake/configs/posix_bebop_default.cmake delete mode 100644 cmake/configs/posix_eagle_default.cmake delete mode 100644 cmake/configs/posix_eagle_hil.cmake delete mode 100644 cmake/configs/posix_eagle_legacy.cmake delete mode 100644 cmake/configs/posix_eagle_muorb.cmake delete mode 100644 cmake/configs/posix_excelsior_default.cmake delete mode 100644 cmake/configs/posix_excelsior_legacy.cmake delete mode 100644 cmake/configs/posix_ocpoc_cross.cmake delete mode 100644 cmake/configs/posix_ocpoc_ubuntu.cmake delete mode 100644 cmake/configs/posix_rpi_common.cmake delete mode 100644 cmake/configs/posix_rpi_cross.cmake delete mode 100644 cmake/configs/posix_rpi_cross_no_shield.cmake delete mode 100644 cmake/configs/posix_rpi_native.cmake delete mode 100644 cmake/configs/posix_sdflight_default.cmake delete mode 100644 cmake/configs/posix_sdflight_legacy.cmake delete mode 100644 cmake/configs/posix_sdflight_rtps.cmake delete mode 100644 cmake/configs/posix_sitl_broadcast.cmake delete mode 100644 cmake/configs/posix_sitl_default.cmake delete mode 100644 cmake/configs/posix_sitl_rtps.cmake delete mode 100644 cmake/configs/posix_sitl_test.cmake delete mode 100644 cmake/configs/qurt_eagle_default.cmake delete mode 100644 cmake/configs/qurt_eagle_hello.cmake delete mode 100644 cmake/configs/qurt_eagle_hil.cmake delete mode 100644 cmake/configs/qurt_eagle_legacy.cmake delete mode 100644 cmake/configs/qurt_eagle_muorb.cmake delete mode 100644 cmake/configs/qurt_eagle_test.cmake delete mode 100644 cmake/configs/qurt_eagle_travis.cmake delete mode 100644 cmake/configs/qurt_excelsior_default.cmake delete mode 100644 cmake/configs/qurt_excelsior_legacy.cmake delete mode 100644 cmake/configs/qurt_sdflight_default.cmake delete mode 100644 cmake/configs/qurt_sdflight_legacy.cmake rename cmake/{common => }/coverage.cmake (100%) create mode 100644 cmake/px4_add_board.cmake rename cmake/{common => }/px4_base.cmake (85%) rename cmake/{common => }/px4_git.cmake (99%) create mode 100644 cmake/px4_make_uavcan_bootloader.cmake rename cmake/{common => }/px4_metadata.cmake (98%) rename cmake/{common => }/sanitizers.cmake (100%) delete mode 100644 cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake delete mode 100644 cmake/toolchains/Toolchain-arm-none-eabi.cmake delete mode 100644 cmake/toolchains/Toolchain-arm-xilinx-linux-gnueabi.cmake delete mode 100644 cmake/toolchains/Toolchain-gcc-arm-linux-gnueabihf.cmake delete mode 100644 platforms/nuttx/Images/px4flow-v2.prototype delete mode 100644 platforms/nuttx/Images/zubaxgnss-v1.prototype rename platforms/nuttx/{nuttx-configs => NuttX}/Make.defs.in (79%) rename platforms/nuttx/{nuttx-configs/aerocore2/include => NuttX}/nsh_romfsimg.h (100%) delete mode 100755 platforms/nuttx/NuttX/tools/new_nuttx_px_config.sh create mode 100644 platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m3.cmake create mode 100644 platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m4.cmake create mode 100644 platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m7.cmake create mode 100644 platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake create mode 100644 platforms/nuttx/cmake/px4_add_board_os.cmake delete mode 100644 platforms/nuttx/nuttx-configs/auav-x21/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/av-x-v1/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/crazyflie/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/mindpx-v2/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/nxphlite-v3/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/omnibus-f4sd/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/Kconfig delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/include/nsh_romfsimg.h delete mode 100755 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/nsh/setenv.sh delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/flash-dtcm.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/flash-sram.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/gnu-elf.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/kernel-space.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/memory.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-same70xplained-v1/scripts/user-space.ld delete mode 100644 platforms/nuttx/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4flow-v2/Kconfig delete mode 100755 platforms/nuttx/nuttx-configs/px4flow-v2/include/README.txt delete mode 100644 platforms/nuttx/nuttx-configs/px4flow-v2/nsh/defconfig delete mode 100644 platforms/nuttx/nuttx-configs/px4flow-v2/scripts/ld.script delete mode 100644 platforms/nuttx/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4fmu-v4pro/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4fmu-v5/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/px4nucleoF767ZI-v1/include/nsh_romfsimg.h delete mode 100644 platforms/nuttx/nuttx-configs/zubaxgnss-v1/Kconfig delete mode 100755 platforms/nuttx/nuttx-configs/zubaxgnss-v1/include/README.txt delete mode 100755 platforms/nuttx/nuttx-configs/zubaxgnss-v1/include/board.h delete mode 100644 platforms/nuttx/nuttx-configs/zubaxgnss-v1/nsh/defconfig delete mode 100644 platforms/nuttx/nuttx-configs/zubaxgnss-v1/scripts/ld.script create mode 100644 platforms/posix/cmake/Platform/Linux-arm-linux-gnueabihf-cortex-a53.cmake create mode 100644 platforms/posix/cmake/Platform/Linux-arm-linux-gnueabihf-cortex-a8.cmake create mode 100644 platforms/posix/cmake/Platform/Linux-arm-linux-gnueabihf-cortex-a9.cmake create mode 100644 platforms/posix/cmake/Toolchain-arm-linux-gnueabihf.cmake rename {cmake/toolchains => platforms/posix/cmake}/Toolchain-native.cmake (100%) create mode 100644 platforms/posix/cmake/px4_add_board_os.cmake create mode 100644 platforms/qurt/cmake/px4_add_board_os.cmake delete mode 100644 src/drivers/boards/px4flow-v2/board_config.h delete mode 100644 src/drivers/boards/px4flow-v2/init.c delete mode 100644 src/drivers/boards/px4flow-v2/led.c delete mode 100644 src/drivers/boards/zubaxgnss-v1/board_config.h delete mode 100644 src/drivers/boards/zubaxgnss-v1/gnss_init.c delete mode 100644 src/drivers/boards/zubaxgnss-v1/gnss_led.c rename src/{drivers/boards/px4flow-v2 => modules/commander/failure_detector}/CMakeLists.txt (93%) diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests index 25a3aafb9cea..2d092fec8649 100644 --- a/.ci/Jenkinsfile-SITL_tests +++ b/.ci/Jenkinsfile-SITL_tests @@ -30,7 +30,7 @@ pipeline { sh('make distclean') sh "ccache -z" sh "git fetch --tags" - sh('make posix_sitl_default') + sh('make px4sitl_default') sh "ccache -s" } } @@ -47,8 +47,8 @@ pipeline { stage('unit tests') { steps { sh 'export' - sh 'make posix_sitl_default test_results_junit' - junit 'build/posix_sitl_default/JUnitTestResults.xml' + sh 'make px4sitl_default test_results_junit' + junit 'build/px4sitl_default/JUnitTestResults.xml' } } @@ -56,7 +56,7 @@ pipeline { steps { sh 'export' sh "ccache -z" - sh('make posix_sitl_default sitl_gazebo') + sh('make px4sitl_default sitl_gazebo') sh "ccache -s" } } @@ -64,8 +64,8 @@ pipeline { stage('package') { steps { sh 'export' - sh('make posix_sitl_default package') - stash(name: "px4_sitl_package", includes: "build/posix_sitl_default/*.bz2") + sh('make px4sitl_default package') + stash(name: "px4_sitl_package", includes: "build/px4sitl_default/*.bz2") } } @@ -176,7 +176,7 @@ pipeline { steps { sh 'export' sh 'make distclean' - sh 'make posix_sitl_default test_results_junit' + sh 'make px4sitl_default test_results_junit' withCredentials([string(credentialsId: 'FIRMWARE_CODECOV_TOKEN', variable: 'CODECOV_TOKEN')]) { sh 'curl -s https://codecov.io/bash | bash -s - -F unittest' } @@ -221,11 +221,11 @@ def createTestNode(Map test_def) { } unstash('px4_sitl_package') - sh('tar -xjpvf build/posix_sitl_default/px4-posix_sitl_default*.bz2') + sh('tar -xjpvf build/px4sitl_default/px4-px4sitl_default*.bz2') // run test try { - sh('px4-posix_sitl_default*/px4/test/rostest_px4_run.sh ' + test_def.test + ' mission:=' + test_def.mission + ' vehicle:=' + test_def.vehicle) + sh('px4-px4sitl_default*/px4/test/rostest_px4_run.sh ' + test_def.test + ' mission:=' + test_def.mission + ' vehicle:=' + test_def.vehicle) } catch (exc) { // save all test artifacts for debugging @@ -240,7 +240,7 @@ def createTestNode(Map test_def) { // process log data (with python code coverage) try { - sh('coverage run -p px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py .ros/log/*/*.ulg') + sh('coverage run -p px4-px4sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py .ros/log/*/*.ulg') } catch (exc) { // save log analysis artifacts for debugging archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.pdf, .ros/**/*.csv') @@ -249,7 +249,7 @@ def createTestNode(Map test_def) { } // upload log to flight review (https://logs.px4.io/) with python code coverage - sh('coverage run -p px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/log/*/*.ulg') + sh('coverage run -p px4-px4sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/log/*/*.ulg') // upload python code coverage to codecov.io sh 'curl -s https://codecov.io/bash | bash -s - -X gcov -F sitl_python_${STAGE_NAME}' @@ -257,7 +257,7 @@ def createTestNode(Map test_def) { } else { // non code coverage // process ekf log data try { - sh('px4-posix_sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py .ros/log/*/*.ulg') + sh('px4-px4sitl_default*/px4/Tools/ecl_ekf/process_logdata_ekf.py .ros/log/*/*.ulg') } catch (exc) { // save log analysis artifacts for debugging archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.pdf, .ros/**/*.csv') @@ -266,7 +266,7 @@ def createTestNode(Map test_def) { } // upload log to flight review (https://logs.px4.io/) - sh('px4-posix_sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/log/*/*.ulg') + sh('px4-px4sitl_default*/px4/Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/log/*/*.ulg') } if (!test_ok) { diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index a0c90fdc89d9..63d86451a3ce 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -18,8 +18,8 @@ pipeline { snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12" ] - // MAC OSX posix_sitl_default - build_nodes["posix_sitl_default (OSX)"] = { + // MAC OSX px4sitl_default + build_nodes["px4sitl_default (OSX)"] = { node("mac") { withEnv(["CCACHE_BASEDIR=${pwd()}"]) { stage("sitl (OSX)") { @@ -28,7 +28,7 @@ pipeline { sh('export') sh('make distclean') sh('ccache -z') - sh('make posix_sitl_default') + sh('make px4sitl_default') sh('ccache -s') sh('make tests') } @@ -43,7 +43,7 @@ pipeline { } } - // MAC OSX nuttx_px4fmu-v4pro_default + // MAC OSX px4fmu-v4pro_default build_nodes["px4fmu-v4pro_default (OSX)"] = { node("mac") { withEnv(["CCACHE_BASEDIR=${pwd()}"]) { @@ -53,7 +53,7 @@ pipeline { sh('export') sh('make distclean') sh('ccache -z') - sh('make nuttx_px4fmu-v4pro_default') + sh('make px4fmu-v4pro_default') sh('ccache -s') } catch (exc) { @@ -69,46 +69,46 @@ pipeline { // docker builds: def arch_builds = [ - target: ["posix_sitl_default"], + target: ["px4sitl_default"], image: docker_images.arch, archive: false ] def armhf_builds = [ - target: ["posix_ocpoc_ubuntu"], + target: ["aerotennaocpoc_ubuntu"], image: docker_images.armhf, archive: false ] def base_builds = [ - target: ["posix_sitl_rtps"], + target: ["px4sitl_rtps"], image: docker_images.base, archive: false ] def nuttx_builds_archive = [ target: ["px4fmu-v2_default", "px4fmu-v3_default", "px4fmu-v4_default", "px4fmu-v4pro_default", "px4fmu-v5_default", "px4fmu-v5_rtps", "px4fmu-v5_stackcheck", - "aerofc-v1_default", "aerocore2_default", "auav-x21_default", "av-x-v1_default", "crazyflie_default", "mindpx-v2_default", - "nxphlite-v3_default", "omnibus-f4sd_default"], + "intelaerofc-v1_default", "gumstixaerocore2_default", "auavx21_default", "avx-v1_default", "bitcrazecrazyflie_default", "airmindmindpx-v2_default", + "nxphlite-v3_default", "omnibusf4sd_default"], image: docker_images.nuttx, archive: true ] def nuttx_builds_other = [ - target: ["px4-same70xplained-v1_default", "px4-stm32f4discovery_default", "px4cannode-v1_default", - "px4esc-v1_default", "px4nucleoF767ZI-v1_default", "s2740vc-v1_default"], + target: ["atmelsame70xplained_default", "stm32f4discovery_default", "px4cannode-v1_default", + "px4esc-v1_default", "stmnucleo-F767ZI_default", "thiemars2740vc-v1_default"], image: docker_images.nuttx, archive: false ] def rpi_builds = [ - target: ["posix_rpi_cross", "posix_bebop_default"], + target: ["emlidnavio2_cross", "parrotbebop_default"], image: docker_images.rpi, archive: false ] def snapdragon_builds = [ - target: ["qurt_eagle_default", "posix_eagle_default"], + target: ["atlflighteagle_qurt-default", "atlflighteagle_default"], image: docker_images.snapdragon, archive: false ] diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 5825efe7c7cc..e95c743d4b97 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -18,10 +18,10 @@ pipeline { sh 'make distclean' sh 'ccache -z' sh 'git fetch --tags' - sh 'make nuttx_px4fmu-v2_test' + sh 'make px4fmu-v2_test' sh 'make sizes' sh 'ccache -s' - stash includes: 'build/nuttx_px4fmu-v2_test/nuttx_px4fmu-v2_test.elf', name: 'px4fmu-v2_test' + stash includes: 'build/px4fmu-v2_test/px4fmu-v2_test.elf', name: 'px4fmu-v2_test' stash includes: 'Tools/HIL/monitor_firmware_upload.py, Tools/HIL/run_tests.py', name: 'scripts-px4fmu-v2' } post { @@ -43,10 +43,10 @@ pipeline { sh 'make distclean' sh 'ccache -z' sh 'git fetch --tags' - sh 'make nuttx_px4fmu-v4_stackcheck' + sh 'make px4fmu-v4_stackcheck' sh 'make sizes' sh 'ccache -s' - stash includes: 'build/nuttx_px4fmu-v4_stackcheck/nuttx_px4fmu-v4_stackcheck.elf', name: 'px4fmu-v4_stackcheck' + stash includes: 'build/px4fmu-v4_stackcheck/px4fmu-v4_stackcheck.elf', name: 'px4fmu-v4_stackcheck' stash includes: 'Tools/HIL/monitor_firmware_upload.py, Tools/HIL/run_tests.py', name: 'scripts-px4fmu-v4' } post { @@ -82,7 +82,7 @@ pipeline { -ex "attach 1" \ -ex "load" \ -ex "kill" \ - build/nuttx_px4fmu-v2_test/nuttx_px4fmu-v2_test.elf + build/px4fmu-v2_test/px4fmu-v2_test.elf ''' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *Black_Magic_Probe_*-if02` --baudrate 57600' sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *Black_Magic_Probe_*-if02`' @@ -116,7 +116,7 @@ pipeline { -ex "attach 1" \ -ex "load" \ -ex "kill" \ - build/nuttx_px4fmu-v4_stackcheck/nuttx_px4fmu-v4_stackcheck.elf + build/px4fmu-v4_stackcheck/px4fmu-v4_stackcheck.elf ''' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *Black_Magic_Probe_*-if02` --baudrate 57600' sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *Black_Magic_Probe_*-if02`' diff --git a/.gitmodules b/.gitmodules index 20028c815573..112c186ef562 100644 --- a/.gitmodules +++ b/.gitmodules @@ -34,8 +34,8 @@ path = src/lib/ecl url = https://github.com/PX4/ecl.git branch = master -[submodule "cmake/cmake_hexagon"] - path = cmake/cmake_hexagon +[submodule "boards/atlflight/cmake_hexagon"] + path = boards/atlflight/cmake_hexagon url = https://github.com/ATLFlight/cmake_hexagon.git branch = master [submodule "src/drivers/gps/devices"] diff --git a/.travis.yml b/.travis.yml index fcc1e11f63e6..53399ae06e4e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -28,5 +28,5 @@ addons: description: "Build submitted via Travis CI" notification_email: ci@px4.io build_command_prepend: "make distclean" - build_command: "make posix_sitl_default" + build_command: "make px4sitl_default" branch_pattern: coverity_scan diff --git a/CMakeLists.txt b/CMakeLists.txt index b1e00ca630a6..fce4003d0d3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,7 @@ # * Common functions should be included in px_base.cmake. # # * OS/ board specific fucntions should be include in -# px_impl_${OS}.cmake or px4_impl_${OS}_${BOARD}.cmake. +# px_impl_${PX4_PLATFORM}.cmake or px4_impl_${PX4_PLATFORM}_${PX4_BOARD}.cmake. # # Formatting # --------------------------------------------------------------------------- @@ -92,9 +92,9 @@ # --------------------------------------------------------------------------- # # * If referencing a string variable, don't put it in quotes. -# Don't do "${OS}" STREQUAL "posix", -# instead type ${OS} STREQUAL "posix". This will throw an -# error when ${OS} is not defined instead of silently +# Don't do "${PX4_PLATFORM}" STREQUAL "posix", +# instead type ${PX4_PLATFORM} STREQUAL "posix". This will throw an +# error when ${PX4_PLATFORM} is not defined instead of silently # evaluating to false. # #============================================================================= @@ -104,12 +104,12 @@ cmake_minimum_required(VERSION 3.2 FATAL_ERROR) set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}") -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/cmake") +list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake) #============================================================================= # git # -include(common/px4_git) +include(px4_git) execute_process( COMMAND git describe --always --tags @@ -132,13 +132,90 @@ define_property(GLOBAL PROPERTY PX4_MODULE_PATHS # configuration # -set(CONFIG "posix_sitl_default" CACHE STRING "desired configuration") +set(CONFIG "px4sitl_default" CACHE STRING "desired configuration") -string(REPLACE "_" ";" config_args ${CONFIG}) -list(GET config_args 0 OS) -list(GET config_args 1 BOARD) -list(GET config_args 2 LABEL) +set(config_module_list) +set(config_df_driver_list) +# find PX4 config +# look for in tree board config that matches CONFIG input +if(NOT PX4_CONFIG_FILE) + + file(GLOB_RECURSE board_configs + RELATIVE "${PX4_SOURCE_DIR}/boards" + "boards/*.cmake" + ) + + set(PX4_CONFIGS ${board_configs} CACHE STRINGS "PX4 board configs" FORCE) + + foreach(filename ${board_configs}) + # parse input CONFIG into components to match with existing in tree configs + # the platform prefix (eg nuttx_) is historical, and removed if present + string(REPLACE ".cmake" "" filename_stripped ${filename}) + string(REPLACE "/" ";" config ${filename_stripped}) + list(LENGTH config config_len) + + if(${config_len} EQUAL 3) + + + list(GET config 0 vendor) + list(GET config 1 model) + list(GET config 2 label) + + set(board "${vendor}${model}") + + # __