diff --git a/.gitmodules b/.gitmodules index 178cf86771..744dcd2787 100644 --- a/.gitmodules +++ b/.gitmodules @@ -25,3 +25,6 @@ [submodule "unittests/googletest"] path = unittests/googletest url = https://github.com/google/googletest.git +[submodule "src/lib/matrix"] + path = src/lib/matrix + url = https://github.com/PX4/Matrix.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 05a3cd59b3..6b395b6261 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") +px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") add_custom_target(submodule_clean WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index b27dc9b3e3..dfd94992af 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -16,9 +16,9 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_P 0.05 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.002 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.08 param set MC_PITCHRATE_I 0.1 @@ -27,8 +27,7 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 - param set PWM_MIN 1075 - param set PWM_THR_MIN 0.06 - param set PWM_MANTHR_MIN 0.06 + param set PWM_MIN 980 + param set MPC_THR_MIN 0.06 + param set MPC_MANTHR_MIN 0.06 fi - diff --git a/Tools/jMAVSim b/Tools/jMAVSim index fd9ed5df04..96029523d1 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit fd9ed5df0496d42f20b91cb405639ceccbcf9e17 +Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml new file mode 100644 index 0000000000..2f164ee52b --- /dev/null +++ b/Tools/parameters_injected.xml @@ -0,0 +1,138 @@ + + + 3 + + + Speed controller bandwidth + Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness. + Hertz + 10 + 250 + + + Reverse direction + Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction. + 0 + 1 + + + Speed (RPM) controller gain + Speed (RPM) controller gain. Determines controller + aggressiveness; units are amp-seconds per radian. Systems with + higher rotational inertia (large props) will need gain increased; + systems with low rotational inertia (small props) may need gain + decreased. Higher values result in faster response, but may result + in oscillation and excessive overshoot. Lower values result in a + slower, smoother response. + amp-seconds per radian + 3 + 0.00 + 1.00 + + + Idle speed (e Hz) + Idle speed (e Hz) + Hertz + 3 + 0.0 + 100.0 + + + Spin-up rate (e Hz/s) + Spin-up rate (e Hz/s) + Hz/s + 5 + 1000 + + + Index of this ESC in throttle command messages. + Index of this ESC in throttle command messages. + Index + 0 + 15 + + + Extended status ID + Extended status ID + 1 + 1000000 + + + Extended status interval (µs) + Extended status interval (µs) + µs + 0 + 1000000 + + + ESC status interval (µs) + ESC status interval (µs) + µs + 1000000 + + + Motor current limit in amps + Motor current limit in amps. This determines the maximum + current controller setpoint, as well as the maximum allowable + current setpoint slew rate. This value should generally be set to + the continuous current rating listed in the motor’s specification + sheet, or set equal to the motor’s specified continuous power + divided by the motor voltage limit. + Amps + 3 + 1 + 80 + + + Motor Kv in RPM per volt + Motor Kv in RPM per volt. This can be taken from the motor’s + specification sheet; accuracy will help control performance but + some deviation from the specified value is acceptable. + RPM/v + 0 + 4000 + + + READ ONLY: Motor inductance in henries. + READ ONLY: Motor inductance in henries. This is measured on start-up. + henries + 3 + + + Number of motor poles. + Number of motor poles. Used to convert mechanical speeds to + electrical speeds. This number should be taken from the motor’s + specification sheet. + Poles + 2 + 40 + + + READ ONLY: Motor resistance in ohms + READ ONLY: Motor resistance in ohms. This is measured on start-up. When + tuning a new motor, check that this value is approximately equal + to the value shown in the motor’s specification sheet. + Ohms + 3 + + + Acceleration limit (V) + Acceleration limit (V) + Volts + 3 + 0.01 + 1.00 + + + Motor voltage limit in volts + Motor voltage limit in volts. The current controller’s + commanded voltage will never exceed this value. Note that this may + safely be above the nominal voltage of the motor; to determine the + actual motor voltage limit, divide the motor’s rated power by the + motor current limit. + Volts + 3 + 0 + + + \ No newline at end of file diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index b072ab79f8..b37cdb0627 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -18,10 +18,14 @@ def indent(elem, level=0): class XMLOutput(): - def __init__(self, groups, board): + def __init__(self, groups, board, inject_xml_file_name): xml_parameters = ET.Element("parameters") xml_version = ET.SubElement(xml_parameters, "version") xml_version.text = "3" + importtree = ET.parse(inject_xml_file_name) + injectgroups = importtree.getroot().findall("group") + for igroup in injectgroups: + xml_parameters.append(igroup) last_param_name = "" board_specific_param_set = False for group in groups: diff --git a/Tools/px_generate_params.py b/Tools/px_generate_params.py index 3df124f523..f1877987ad 100755 --- a/Tools/px_generate_params.py +++ b/Tools/px_generate_params.py @@ -29,7 +29,7 @@ start_name = "" end_name = "" for group in root: - if group.tag == "group": + if group.tag == "group" and "no_code_generation" not in group.attrib: header += """ /***************************************************************** * %s @@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = { """ i=0 for group in root: - if group.tag == "group": + if group.tag == "group" and "no_code_generation" not in group.attrib: + src += """ /***************************************************************** * %s diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index c2024dc726..3a11fc5c50 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,12 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-i", "--inject-xml", + nargs='?', + const="../Tools/parameters_injected.xml", + metavar="FILENAME", + help="Inject additional param XML file" + " (default FILENAME: ../Tools/parameters_injected.xml)") parser.add_argument("-b", "--board", nargs='?', const="", @@ -124,7 +130,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board) + out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml) out.Save(args.xml) # Output to DokuWiki tables diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 1fa1efbd90..a7984c66dd 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image """ from __future__ import print_function -import argparse +import argparse, re import os @@ -64,7 +64,7 @@ def main(): # read file line by line pruned_content = "" - with open(file_path, "r") as f: + with open(file_path, "rU") as f: for line in f: # handle mixer files differently than startup files @@ -76,6 +76,7 @@ def main(): pruned_content += line # overwrite old scratch file with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) f.write(pruned_content.encode("ascii", errors='strict')) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 7d855cd385..d362e661b4 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 7d855cd3853c6c6c41237b114bbc1fd9a3cf102f +Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 521ebeef21..8767f9f841 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -56,8 +56,8 @@ then export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models - # Disable online model lookup since this is quite experimental and unstable - export GAZEBO_MODEL_DATABASE_URI="" + # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. + # export GAZEBO_MODEL_DATABASE_URI="" export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build @@ -83,6 +83,12 @@ then elif [ "$debugger" == "gdb" ] then gdb --args mainapp ../../../../${rc_script}_${program} +elif [ "$debugger" == "ddd" ] +then + ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} +elif [ "$debugger" == "valgrind" ] +then + valgrind ./mainapp ../../../../${rc_script}_${program} else ./mainapp ../../../../${rc_script}_${program} fi diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index eb3b9c68fc..8a3a844886 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -622,7 +622,7 @@ function(px4_add_common_flags) ) list(APPEND added_include_dirs - src/lib/eigen + src/lib/matrix ) set(added_link_dirs) # none used currently @@ -741,7 +741,7 @@ function(px4_generate_parameters_xml) ) add_custom_command(OUTPUT ${OUT} COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py - -s ${path} --board CONFIG_ARCH_${BOARD} --xml + -s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml DEPENDS ${param_src_files} ) set(${OUT} ${${OUT}} PARENT_SCOPE) diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index b0a8158070..581b82e827 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -36,6 +36,7 @@ set(config_module_list modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator + modules/vtol_att_control modules/mc_pos_control modules/mc_att_control modules/mc_pos_control_multiplatform diff --git a/cmake/ros-CMakeLists.txt b/cmake/ros-CMakeLists.txt index aa42fb6ff4..092a968991 100644 --- a/cmake/ros-CMakeLists.txt +++ b/cmake/ros-CMakeLists.txt @@ -81,6 +81,7 @@ add_message_files( offboard_control_mode.msg vehicle_force_setpoint.msg distance_sensor.msg + control_state.msg ) ## Generate services in the 'srv' folder @@ -135,6 +136,7 @@ include_directories( ${CMAKE_BINARY_DIR}/src/modules src/ src/lib + src/lib/matrix ${EIGEN_INCLUDE_DIRS} integrationtests ) diff --git a/msg/control_state.msg b/msg/control_state.msg new file mode 100644 index 0000000000..4f130353d3 --- /dev/null +++ b/msg/control_state.msg @@ -0,0 +1,19 @@ +# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */ +uint64 timestamp # in microseconds since system start +float32 x_acc # X acceleration in body frame +float32 y_acc # Y acceleration in body frame +float32 z_acc # Z acceleration in body frame +float32 x_vel # X velocity in body frame +float32 y_vel # Y velocity in body frame +float32 z_vel # Z velocity in body frame +float32 x_pos # X position in local frame +float32 y_pos # Y position in local frame +float32 z_pos # z position in local frame +float32 airspeed # Airspeed, estimated +float32[3] vel_variance # Variance in body velocity estimate +float32[3] pos_variance # Variance in local position estimate +float32[4] q # Attitude Quaternion +float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) +float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) +float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) +float32 horz_acc_mag # Magnitude of the horizontal acceleration diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e575b9d5e3..c91672c0be 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -120,6 +120,8 @@ bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation @@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. +bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events @@ -179,3 +182,4 @@ bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check bool circuit_breaker_engaged_gpsfailure_check +bool cb_usb diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 0b52ca48ff..155173db5f 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -82,6 +82,9 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +/* SPI4--Ramtron */ +#define PX4_SPI_BUS_RAMTRON 4 + /* Nominal chip selects for devices on SPI bus #3 */ #define PX4_SPIDEV_ACCEL_MAG 0 #define PX4_SPIDEV_GYRO 1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0f1f2fd4a2..ea60df9100 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -113,6 +113,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d82ef9e08..64d9dced4a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + // // XXX bring up the EXTI pins again // stm32_configgpio(GPIO_GYRO_DRDY); // stm32_configgpio(GPIO_MAG_DRDY); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 719bacb41f..2cabe98a23 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / - ((_rssi_pwm_max - _rssi_pwm_min) / 100); + int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 64f4775c33..fb7490ffe2 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -33,7 +33,7 @@ add_custom_target(run_config add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) - foreach(debugger none gdb lldb) + foreach(debugger none gdb lldb ddd valgrind) foreach(model none iris vtol) if (debugger STREQUAL "none") if (model STREQUAL "none") diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 737d9ea1a3..299811ea43 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -43,6 +43,7 @@ #include DataValidator::DataValidator(DataValidator *prev_sibling) : + _error_mask(ERROR_FLAG_NO_ERROR), _time_last(0), _timeout_interval(20000), _event_count(0), @@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in float DataValidator::confidence(uint64_t timestamp) { + float ret = 1.0f; + /* check if we have any data */ if (_time_last == 0) { - return 0.0f; + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; } - - /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { - return 0.0f; + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; } /* we got the exact same sensor value N times in a row */ if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - return 0.0f; + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; } - - /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { - return 0.0f; + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; } /* cap error density counter at window size */ if (_error_density > ERROR_DENSITY_WINDOW) { + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; } - - /* return local error density for last N measurements */ - return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); -} - -int -DataValidator::priority() -{ - return _priority; + + /* no critical errors */ + if(ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + } + + return ret; } void diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index dde9cb51aa..31f6d9d989 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -91,7 +91,18 @@ public: * Get the priority of this validator * @return the stored priority */ - int priority(); + int priority() { return (_priority); } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() { return (_error_mask); } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } /** * Get the RMS values of this validator @@ -111,9 +122,20 @@ public: * @param timeout_interval_us The timeout interval in microseconds */ void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); private: static const unsigned _dimensions = 3; + uint32_t _error_mask; /**< sensor error state */ uint64_t _time_last; /**< last timestamp */ uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ uint64_t _event_count; /**< total data counter */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index ac4bcc8b9b..8bf060cf65 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) bool true_failsafe = true; - /* check wether the switch was a failsafe or preferring a higher priority sensor */ + /* check whether the switch was a failsafe or preferring a higher priority sensor */ if (pre_check_prio != -1 && pre_check_prio < max_priority && fabsf(pre_check_confidence - max_confidence) < 0.1f) { /* this is not a failover */ true_failsafe = false; + /* reset error flags, this is likely a hotplug sensor coming online late */ + best->reset_state(); } /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ @@ -199,17 +201,25 @@ void DataValidatorGroup::print() { /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); - DataValidator *next = _first; unsigned i = 0; while (next != nullptr) { if (next->used()) { - ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + uint32_t flags = next->state(); + + ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + next->print(); } next = next->sibling(); @@ -222,3 +232,35 @@ DataValidatorGroup::failover_count() { return _toggle_count; } + +int +DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { + return i; + } + next = next->sibling(); + i++; + } + return -1; +} + +uint32_t +DataValidatorGroup::failover_state() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { + return next->state(); + } + next = next->sibling(); + i++; + } + return DataValidator::ERROR_FLAG_NO_ERROR; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index 3756be2638..855c3ed4ae 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -80,6 +80,20 @@ public: * @return the number of failovers */ unsigned failover_count(); + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); /** * Print the validator value diff --git a/src/lib/launchdetection/CMakeLists.txt b/src/lib/launchdetection/CMakeLists.txt index 7ea0c18788..b396169a45 100644 --- a/src/lib/launchdetection/CMakeLists.txt +++ b/src/lib/launchdetection/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( SRCS LaunchDetector.cpp CatapultLaunchMethod.cpp - launchdetection_params.c DEPENDS platforms__common ) diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 5159f2fcb2..e5070eeaaf 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Catapult launch detection parameters, accessible via MAVLink * diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 38322e56a7..54a1b87ee0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,7 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include "modules/local_position_estimator/matrix/src/Matrix.hpp" +#include "matrix/math.hpp" #endif #include @@ -339,9 +339,8 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - matrix::Matrix Me(this->arm_mat.pData); - matrix::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea - Matrix res(MyInverse.data()); + matrix::SquareMatrix Me = matrix::Matrix(this->arm_mat.pData); + Matrix res(Me.I().data()); return res; #endif } diff --git a/src/lib/matrix b/src/lib/matrix new file mode 160000 index 0000000000..7656385ea1 --- /dev/null +++ b/src/lib/matrix @@ -0,0 +1 @@ +Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b 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 a9ffbe0c42..4b4fba54c8 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -54,8 +54,11 @@ #include #include #include +#include #include #include +#include +#include #include #include @@ -117,22 +120,28 @@ private: int _sensors_sub = -1; int _params_sub = -1; + int _vision_sub = -1; + int _mocap_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; + orb_advert_t _ctrl_state_pub = nullptr; struct { param_t w_acc; param_t w_mag; + param_t w_ext_hdg; param_t w_gyro_bias; param_t mag_decl; param_t mag_decl_auto; param_t acc_comp; param_t bias_max; param_t vibe_thresh; + param_t ext_hdg_mode; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; float _w_mag = 0.0f; + float _w_ext_hdg = 0.0f; float _w_gyro_bias = 0.0f; float _mag_decl = 0.0f; bool _mag_decl_auto = false; @@ -140,11 +149,18 @@ private: float _bias_max = 0.0f; float _vibration_warning_threshold = 1.0f; hrt_abstime _vibration_warning_timestamp = 0; + int _ext_hdg_mode = 0; Vector<3> _gyro; Vector<3> _accel; Vector<3> _mag; + vision_position_estimate_s _vision = {}; + Vector<3> _vision_hdg; + + att_pos_mocap_s _mocap = {}; + Vector<3> _mocap_hdg; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -168,6 +184,7 @@ private: bool _data_good = false; bool _failsafe = false; bool _vibration_warning = false; + bool _ext_hdg_good = false; int _mavlink_fd = -1; @@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_mag(3), _lp_roll_rate(250.0f, 18.0f), _lp_pitch_rate(250.0f, 18.0f), - _lp_yaw_rate(250, 10.0f) + _lp_yaw_rate(250.0f, 10.0f) { _voter_mag.set_timeout(200000); _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); + _params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.mag_decl = param_find("ATT_MAG_DECL"); _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); } /** @@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) void AttitudeEstimatorQ::task_main() { _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + + _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -325,10 +348,17 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + /* ignore empty fields */ + if (sensors.accelerometer_timestamp[i] > 0) { + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + } + + /* ignore empty fields */ + if (sensors.magnetometer_timestamp[i] > 0) { + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } } int best_gyro, best_accel, best_mag; @@ -346,12 +376,48 @@ void AttitudeEstimatorQ::task_main() _data_good = true; - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + if (!_failsafe) { + uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; - _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + flags = _voter_gyro.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!", + _voter_gyro.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + flags = _voter_accel.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!", + _voter_accel.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + flags = _voter_mag.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!", + _voter_mag.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || @@ -374,6 +440,47 @@ void AttitudeEstimatorQ::task_main() } } + // Update vision and motion capture heading + bool vision_updated = false; + orb_check(_vision_sub, &vision_updated); + + bool mocap_updated = false; + orb_check(_mocap_sub, &mocap_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision); + math::Quaternion q(_vision.q); + + math::Matrix<3, 3> Rvis = q.to_dcm(); + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rvis is Rwr (robot respect to world) while v is respect to world. + // Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _vision_hdg = Rvis.transposed() * v; + } + + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); + math::Quaternion q(_mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rmoc is Rwr (robot respect to world) while v is respect to world. + // Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _mocap_hdg = Rmoc.transposed() * v; + } + + // Check for timeouts on data + if (_ext_hdg_mode == 1) { + _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); + + } else if (_ext_hdg_mode == 2) { + _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000); + } + bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); @@ -431,12 +538,9 @@ void AttitudeEstimatorQ::task_main() att.pitch = euler(1); att.yaw = euler(2); - /* the complimentary filter should reflect the true system - * state, but we need smoother outputs for the control system - */ - att.rollspeed = _lp_roll_rate.apply(_rates(0)); - att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); - att.yawspeed = _lp_yaw_rate.apply(_rates(2)); + att.rollspeed = _rates(0); + att.pitchspeed = _rates(1); + att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { att.g_comp[i] = _accel(i) - _pos_acc(i); @@ -461,6 +565,34 @@ void AttitudeEstimatorQ::task_main() } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } + + struct control_state_s ctrl_state = {}; + + ctrl_state.timestamp = sensors.timestamp; + + /* Attitude quaternions for control state */ + ctrl_state.q[0] = _q(0); + + ctrl_state.q[1] = _q(1); + + ctrl_state.q[2] = _q(2); + + ctrl_state.q[3] = _q(3); + + /* Attitude rates for control state */ + ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + + ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2)); + + /* Publish to control state topic */ + if (_ctrl_state_pub == nullptr) { + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + + } else { + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); + } } } @@ -478,6 +610,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_mag, &_w_mag); + param_get(_params_handles.w_ext_hdg, &_w_ext_hdg); param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); @@ -490,6 +623,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); + param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); } } @@ -545,12 +679,34 @@ bool AttitudeEstimatorQ::update(float dt) // Angular rate of correction Vector<3> corr; - // Magnetometer correction - // Project mag field vector to global frame and extract XY component - Vector<3> mag_earth = _q.conjugate(_mag); - float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); - // Project magnetometer correction to body frame - corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + if (_ext_hdg_mode > 0 && _ext_hdg_good) { + if (_ext_hdg_mode == 1) { + // Vision heading correction + // Project heading to global frame and extract XY component + Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg); + float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg; + } + + if (_ext_hdg_mode == 2) { + // Mocap heading correction + // Project heading to global frame and extract XY component + Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg); + float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg; + } + } + + if (_ext_hdg_mode == 0 || !_ext_hdg_good) { + // Magnetometer correction + // Project mag field vector to global frame and extract XY component + Vector<3> mag_earth = _q.conjugate(_mag); + float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); + // Project magnetometer correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + } // Accelerometer correction // Project 'k' unit vector of earth frame to body frame diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d3959db4cf..41b62f04e1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); +/** + * Complimentary filter external heading weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f); + /** * Complimentary filter gyroscope bias weight * @@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); */ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); +/** + * External heading usage mode (from Motion capture/Vision) + * Set to 1 to use heading estimate from vision. + * Set to 2 to use heading from motion capture. + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); + /** * Enable acceleration compensation based on GPS * velocity. diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..8093ef56ba 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid) return !calibration_found; } -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); } @@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; @@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); success = false; goto out; @@ -154,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } @@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; goto out; @@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); success = false; goto out; @@ -200,13 +200,15 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); /* this is frickin' fatal */ success = false; goto out; } } else { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + } /* this is frickin' fatal */ success = false; goto out; @@ -219,7 +221,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -229,8 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } } return false; @@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -259,7 +267,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -269,8 +277,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } } return false; @@ -294,7 +304,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return success; } -static bool airspeedCheck(int mavlink_fd, bool optional) +static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) { bool success = true; int ret; @@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + } success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + } // XXX do not make this fatal yet } @@ -319,7 +333,7 @@ out: return success; } -static bool gnssCheck(int mavlink_fd) +static bool gnssCheck(int mavlink_fd, bool report_fail) { bool success = true; @@ -341,8 +355,10 @@ static bool gnssCheck(int mavlink_fd) } //Report failure to detect module - if(!success) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if (!success) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } } px4_close(gpsSub); @@ -350,7 +366,7 @@ static bool gnssCheck(int mavlink_fd) } bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures) { bool failed = false; @@ -365,7 +381,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_mag_count); int device_id = -1; - if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -376,7 +392,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found"); + } failed = true; } } @@ -392,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_accel_count); int device_id = -1; - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) { failed = true; } @@ -403,7 +421,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + } failed = true; } } @@ -419,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_gyro_count); int device_id = -1; - if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -430,7 +450,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + } failed = true; } } @@ -446,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_baro_count); int device_id = -1; - if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -458,29 +480,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational"); + } failed = true; } } /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_fd, true)) { + if (!airspeedCheck(mavlink_fd, true, reportFailures)) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rc_calibration_check(mavlink_fd) != OK) { - mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed"); + } failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd)) { + if(!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index b6423a4d9a..058d6b8956 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -67,7 +67,7 @@ namespace Commander * true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index eb2bcd650e..4139a949e0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -68,7 +68,7 @@ static const char *sensor_name = "dpress"; static void feedback_calibration_failed(int mavlink_fd) { sleep(5); - mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) @@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd) const unsigned maxcount = 2400; /* give directions */ - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = (maxcount * 2) / 3; @@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } px4_close(fd); @@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } - mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { @@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } @@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); goto error_return; } @@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; @@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); feedback_calibration_failed(mavlink_fd); goto error_return; } else { - mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); normal_return: diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..83e79b77fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 +#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */ + #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 @@ -360,6 +362,8 @@ int commander_main(int argc, char *argv[]) calib_ret = do_level_calibration(mavlink_fd); } else if (!strcmp(argv[2], "esc")) { calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if (!strcmp(argv[2], "airspeed")) { + calib_ret = do_airspeed_calibration(mavlink_fd); } else { warnx("argument %s unsupported.", argv[2]); } @@ -377,23 +381,31 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - int checkres = prearm_check(&status, mavlink_fd_local); + int checkres = 0; + checkres = preflight_check(&status, mavlink_fd_local, false); + warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); + checkres = preflight_check(&status, mavlink_fd_local, true); + warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); - warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(true, mavlink_fd_local, "command line"); - warnx("note: not updating home position on commandline arming!"); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + } else { + warnx("arming failed"); + } px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(false, mavlink_fd_local, "command line"); + if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) { + warnx("rejected disarm"); + } px4_close(mavlink_fd_local); return 0; } @@ -891,6 +903,7 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; bool was_armed = false; @@ -1020,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; + + status.condition_system_prearm_error_reported = true; + status.condition_system_hotplug_timeout = false; status.counter++; status.timestamp = hrt_absolute_time(); @@ -1101,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -1243,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check int32_t rc_in_off = 0; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; param_get(_param_autostart_id, &autostart_id); param_get(_param_rc_in_off, &rc_in_off); status.rc_input_mode = rc_in_off; @@ -1251,14 +1268,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune - } } commander_boot_timestamp = hrt_absolute_time(); @@ -1354,11 +1367,11 @@ int commander_thread_main(int argc, char *argv[]) if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } /* re-check RC calibration */ - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ @@ -1446,6 +1459,8 @@ int commander_thread_main(int argc, char *argv[]) !armed.armed) { bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -1456,11 +1471,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); } else { /* check sensors also */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -1686,10 +1701,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "LANDING DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED"); } else { - mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED"); } } @@ -2177,9 +2192,13 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - /* only report a regain */ + /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); + } else if (telemetry_last_dl_loss[i] == 0){ + /* do not report a new data link in order to not spam the user */ + status.data_link_found_new = true; + status_changed = true; } telemetry_lost[i] = false; @@ -2189,6 +2208,10 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; + if(status.data_link_found_new) { + status.data_link_found_new = false; + status_changed = true; + } } } else { @@ -2414,7 +2437,7 @@ int commander_thread_main(int argc, char *argv[]) } else { set_tune(TONE_STOP_TUNE); } - + /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { @@ -2425,6 +2448,21 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + + /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ + hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + + if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) { + set_tune_override(TONE_GPS_WARNING_TUNE); + sensor_fail_tune_played = true; + status_changed = true; + } + + /* update timeout flag */ + if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) { + status.condition_system_hotplug_timeout = hotplug_timeout; + status_changed = true; + } counter++; @@ -2482,6 +2520,8 @@ get_circuit_breaker_params() { status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.cb_usb = + circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status.circuit_breaker_engaged_enginefailure_check = @@ -2508,19 +2548,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed) { bool set_normal_color = false; - + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* set mode */ if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; + + } else if (!status.condition_system_sensors_initialized && !hotplug_timeout) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; } else { // STANDBY_ERROR and other states rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); @@ -2536,7 +2581,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { rgbled_set_color(RGBLED_COLOR_RED); } else { - if (status_local->condition_home_position_valid) { + if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -3197,6 +3242,7 @@ void *commander_low_prio_loop(void *arg) // so this would be prone to false negatives. bool checkAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -3204,7 +3250,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..bcdb5d3f9d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; -static bool sensor_feedback_provided = false; - transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s */ int prearm_ret = OK; - /* only perform the check if we have to */ + /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = prearm_check(status, mavlink_fd); + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); + } + /* re-run the pre-flight check as long as sensors are failing */ + if (!status->condition_system_sensors_initialized + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || + new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; } /* @@ -171,7 +177,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } @@ -196,10 +202,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } @@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + if ((!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout) || + (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - sensor_feedback_provided = true; + status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; @@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && valid_transition) { - sensor_feedback_provided = false; + status->condition_system_prearm_error_reported = false; + } + if(status->data_link_found_new == true) + { + status->condition_system_prearm_error_reported = false; } /* end of atomic state update */ @@ -380,7 +392,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; @@ -453,7 +465,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta closedir(d); ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); } else { /* failed opening dir */ @@ -490,11 +502,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); #endif } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; @@ -731,24 +743,26 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) { - /* at this point this equals the preflight check, but might add additional - * quantities later. + /* */ + bool reportFailures = false; + reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; } - bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); - - if (status->usb_connected) { - prearm_ok = false; - mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + + if (!status->cb_usb && status->usb_connected && prearm) { + preflight_ok = false; + if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } - return !prearm_ok; + return !preflight_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 084813f8cb..bdb7b13f28 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 421e109eb3..1429979d6d 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -154,12 +155,14 @@ private: int _armedSub; orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _ctrl_state_pub; /**< control state */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; @@ -320,6 +323,12 @@ private: **/ void publishAttitude(); + /** + * @brief + * Publish the system state for control modules + **/ + void publishControlState(); + /** * @brief * Publish local position relative to reference point where filter was initialized diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8080fd9c09..f4fcf78e25 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : /* publications */ _att_pub(nullptr), + _ctrl_state_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), _att{}, + _ctrl_state{}, _gyro{}, _accel{}, _mag{}, @@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish attitude estimations publishAttitude(); + // publish control state + publishControlState(); + // Publish Local Position estimations publishLocalPosition(); @@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude() /* lazily publish the attitude only once available */ if (_att_pub != nullptr) { - /* publish the attitude setpoint */ + /* publish the attitude */ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); } else { @@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude() } } +void AttitudePositionEstimatorEKF::publishControlState() +{ + /* Accelerations in Body Frame */ + _ctrl_state.x_acc = _ekf->accel.x; + _ctrl_state.y_acc = _ekf->accel.y; + _ctrl_state.z_acc = _ekf->accel.z; + + /* Velocity in Body Frame */ + Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); + Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]); + Vector3f v_b = _ekf->Tnb * v_n; + Vector3f v_b_var = _ekf->Tnb * v_n_var; + + _ctrl_state.x_vel = v_b.x; + _ctrl_state.y_vel = v_b.y; + _ctrl_state.z_vel = v_b.z; + + _ctrl_state.vel_variance[0] = v_b_var.x; + _ctrl_state.vel_variance[1] = v_b_var.y; + _ctrl_state.vel_variance[2] = v_b_var.z; + + /* Local Position */ + _ctrl_state.x_pos = _ekf->states[7]; + _ctrl_state.y_pos = _ekf->states[8]; + + // XXX need to announce change of Z reference somehow elegantly + _ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset; + + _ctrl_state.pos_variance[0] = _ekf->P[7][7]; + _ctrl_state.pos_variance[1] = _ekf->P[8][8]; + _ctrl_state.pos_variance[2] = _ekf->P[9][9]; + + /* Attitude */ + _ctrl_state.timestamp = _last_sensor_timestamp; + _ctrl_state.q[0] = _ekf->states[0]; + _ctrl_state.q[1] = _ekf->states[1]; + _ctrl_state.q[2] = _ekf->states[2]; + _ctrl_state.q[3] = _ekf->states[3]; + + /* Airspeed (Groundspeed - Windspeed) */ + _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + + /* Attitude Rates */ + _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; + _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; + _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + + /* Guard from bad data */ + if (!PX4_ISFINITE(_ctrl_state.x_vel) || + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) + { + // bad data, abort publication + return; + } + + /* lazily publish the control state only once available */ + if (_ctrl_state_pub != nullptr) { + /* publish the control state */ + orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state); + + } else { + /* advertise and publish */ + _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state); + } +} + void AttitudePositionEstimatorEKF::publishLocalPosition() { _local_pos.timestamp = _last_sensor_timestamp; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b794b9f90d..d60130786c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -48,6 +48,8 @@ #define M_PI_F static_cast(M_PI) #endif +#define MIN_AIRSPEED_MEAS 5.0f + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the predicted airspeed VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); @@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit() P[13][13] = sq(0.2f*dtIMU); //Wind velocities - P[14][14] = 0.0f; + P[14][14] = 0.01f; P[15][15] = P[14][14]; //Earth magnetic field diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a455cdf8b4..3ed5b3a5a3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -67,7 +66,7 @@ #include #include #include -#include +#include #include #include #include @@ -125,11 +124,10 @@ private: bool _task_running; /**< if true, task is running in its mainloop */ int _control_task; /**< task handle */ - int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ @@ -144,12 +142,11 @@ private: orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ @@ -242,6 +239,11 @@ private: } _parameter_handles; /**< handles for interesting parameters */ + // Rotation matrix and euler angles to extract from control state + math::Matrix<3, 3> _R; + float _roll; + float _pitch; + float _yaw; ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; @@ -269,12 +271,6 @@ private: */ void vehicle_manual_poll(); - - /** - * Check for airspeed updates. - */ - void vehicle_airspeed_poll(); - /** * Check for accel updates. */ @@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), + _ctrl_state_sub(-1), _accel_sub(-1), - _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _debug(false) { /* safely initialize structs */ - _att = {}; + _ctrl_state = {}; _accel = {}; _att_sp = {}; _rates_sp = {}; _manual = {}; - _airspeed = {}; _vcontrol_mode = {}; _actuators = {}; _actuators_airframe = {}; @@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -void -FixedwingAttitudeControl::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - } -} - void FixedwingAttitudeControl::vehicle_accel_poll() { @@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main() * do subscriptions */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _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)); @@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; + fds[1].fd = _ctrl_state_sub; fds[1].events = POLLIN; _task_running = true; @@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main() deltaT = 0.01f; /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main() * Rxy Ryy Rzy -Rzy Ryy Rxy * Rxz Ryz Rzz -Rzz Ryz Rxz * */ - math::Matrix<3, 3> R; //original rotation matrix - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R.set(_att.R); - R_adapted.set(_att.R); + math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); + R_adapted(0, 0) = _R(0, 2); + R_adapted(1, 0) = _R(1, 2); + R_adapted(2, 0) = _R(2, 2); /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); + R_adapted(0, 2) = _R(0, 0); + R_adapted(1, 2) = _R(1, 0); + R_adapted(2, 2) = _R(2, 0); /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation - euler_angles = R_adapted.to_euler(); + euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation /* fill in new attitude data */ - _att.roll = euler_angles(0); - _att.pitch = euler_angles(1); - _att.yaw = euler_angles(2); - PX4_R(_att.R, 0, 0) = R_adapted(0, 0); - PX4_R(_att.R, 0, 1) = R_adapted(0, 1); - PX4_R(_att.R, 0, 2) = R_adapted(0, 2); - PX4_R(_att.R, 1, 0) = R_adapted(1, 0); - PX4_R(_att.R, 1, 1) = R_adapted(1, 1); - PX4_R(_att.R, 1, 2) = R_adapted(1, 2); - PX4_R(_att.R, 2, 0) = R_adapted(2, 0); - PX4_R(_att.R, 2, 1) = R_adapted(2, 1); - PX4_R(_att.R, 2, 2) = R_adapted(2, 2); + _R = R_adapted; + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; + float helper = _ctrl_state.roll_rate; + _ctrl_state.roll_rate = -_ctrl_state.yaw_rate; + _ctrl_state.yaw_rate = helper; } - vehicle_airspeed_poll(); - vehicle_setpoint_poll(); vehicle_accel_poll(); @@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is not updating, we assume the normal average speed */ - if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) { airspeed = _parameters.airspeed_trim; if (nonfinite) { perf_count(_nonfinite_input_perf); } } else { /* prevent numerical drama by requiring 0.5 m/s minimal speed */ - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + airspeed = math::max(0.5f, _ctrl_state.airspeed); } /* @@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main() /* the pilot does not want to change direction, * take straight attitude setpoint from position controller */ - if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) { + if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; } else { roll_sp = (_manual.y * _parameters.man_roll_max) @@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main() } /* Prepare speed_body_u and speed_body_w */ - float speed_body_u = 0.0f; - float speed_body_v = 0.0f; - float speed_body_w = 0.0f; - if(_att.R_valid) { - speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d; - speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d; - speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d; - } else { - if (_debug && loop_counter % 10 == 0) { - warnx("Did not get a valid R\n"); - } - } + float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d; + float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d; + float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d; /* Prepare data for attitude controllers */ struct ECL_ControlData control_input = {}; - control_input.roll = _att.roll; - control_input.pitch = _att.pitch; - control_input.yaw = _att.yaw; - control_input.roll_rate = _att.rollspeed; - control_input.pitch_rate = _att.pitchspeed; - control_input.yaw_rate = _att.yawspeed; + control_input.roll = _roll; + control_input.pitch = _pitch; + control_input.yaw = _yaw; + control_input.roll_rate = _ctrl_state.roll_rate; + control_input.pitch_rate = _ctrl_state.pitch_rate; + control_input.yaw_rate = _ctrl_state.yaw_rate; control_input.speed_body_u = speed_body_u; control_input.speed_body_v = speed_body_v; control_input.speed_body_w = speed_body_w; @@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; + _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a629b9d2cb..f21bfc5f2f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,14 +68,13 @@ #include #include #include -#include #include #include #include #include #include #include -#include +#include #include #include #include @@ -153,8 +152,7 @@ private: int _global_pos_sub; int _pos_sp_triplet_sub; - int _att_sub; /**< vehicle attitude subscription */ - int _airspeed_sub; /**< airspeed subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _control_mode_sub; /**< control mode subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -165,11 +163,10 @@ private: orb_advert_t _tecs_status_pub; /**< TECS status publication */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< control mode */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ @@ -222,6 +219,9 @@ private: float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid math::Matrix<3, 3> _R_nb; ///< current attitude + float _roll; + float _pitch; + float _yaw; ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -356,14 +356,9 @@ private: bool vehicle_manual_control_setpoint_poll(); /** - * Check for airspeed updates. + * Check for changes in control state. */ - bool vehicle_airspeed_poll(); - - /** - * Check for position updates. - */ - void vehicle_attitude_poll(); + void control_state_poll(); /** * Check for accel updates. @@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), - _att_sub(-1), - _airspeed_sub(-1), + _ctrl_state_sub(-1), _control_mode_sub(-1), _vehicle_status_sub(-1), _params_sub(-1), @@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _nav_capabilities_pub(nullptr), /* states */ - _att(), + _ctrl_state(), _att_sp(), _nav_capabilities(), _manual(), - _airspeed(), _control_mode(), _vehicle_status(), _global_pos(), @@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll() } } -bool -FixedwingPositionControl::vehicle_airspeed_poll() -{ - /* check if there is an airspeed update or if it timed out */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - _airspeed_valid = true; - _airspeed_last_valid = hrt_absolute_time(); - - } else { - - /* no airspeed updates for one second */ - if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { - _airspeed_valid = false; - } - } - - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - - return airspeed_updated; -} - bool FixedwingPositionControl::vehicle_manual_control_setpoint_poll() { @@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll() return manual_updated; } - void -FixedwingPositionControl::vehicle_attitude_poll() +FixedwingPositionControl::control_state_poll() { /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); + bool ctrl_state_updated; + orb_check(_ctrl_state_sub, &ctrl_state_updated); - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (ctrl_state_updated) { + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _R_nb(i, j) = PX4_R(_att.R, i, j); + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } } + + /* set rotation matrix and euler angles */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R_nb = q_att.to_dcm(); + + math::Vector<3> euler_angles; + euler_angles = _R_nb.to_euler(); + _roll = euler_angles(0); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); } void @@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) float airspeed; if (_airspeed_valid) { - airspeed = _airspeed.true_airspeed_m_s; + airspeed = _ctrl_state.airspeed; } else { airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; @@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update TECS filters */ if (!_mTecs.getEnabled()) { - _tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, + _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } @@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } else { _tecs.reset_state(); } @@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; /* reset hold yaw */ - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; } else { - target_bearing = _att.yaw; + target_bearing = _yaw; } mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); } -// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); + _l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d); land_noreturn_horizontal = true; @@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } @@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } else { _tecs.reset_state(); } @@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* just switched back from non heading-hold to heading hold */ if (!_hdg_hold_enabled) { _hdg_hold_enabled = true; - _hdg_hold_yaw = _att.yaw; + _hdg_hold_yaw = _yaw; get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); } @@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); - _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + _mTecs.resetDerivatives(_ctrl_state.airspeed); } else { _tecs.reset_state(); } @@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main() // XXX add timestamp check _global_pos_valid = true; - vehicle_attitude_poll(); + control_state_poll(); vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); - vehicle_airspeed_poll(); vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); @@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, limitOverride); } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { @@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, + _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, + _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, pitch_min_rad, pitch_max_rad); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index f3295e1fc9..d1262f9177 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict() Q(X_bz, X_bz) = _pn_b_noise_power.get(); // continuous time kalman filter prediction - Matrix dx = (A * _x + B * _u) * getDt(); + Vector dx = (A * _x + B * _u) * getDt(); // only predict for components we have // valid measurements for @@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow() _flowY += global_speed[1] * dt; // measurement - Vector2f y; + Vector y; y(0) = _flowX; y(1) = _flowY; // residual - Vector2f r = y - C * _x; + Vector r = y - C * _x; // residual covariance, (inverse) Matrix S_I = - (C * _P * C.transpose() + R).inverse(); + inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar() } // measurement - Matrix y; + Vector y; y(0) = (d - _sonarAltHome) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); // residual - Matrix r = y - C * _x; + Vector r = y - C * _x; // residual covariance, (inverse) Matrix S_I = - (C * _P * C.transpose() + R).inverse(); + inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar() void BlockLocalPositionEstimator::correctBaro() { - Matrix y; + Vector y; y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; // baro measurement matrix @@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro() // residual Matrix S_I = - ((C * _P * C.transpose()) + R).inverse(); - Matrix r = y - (C * _x); + inv((C * _P * C.transpose()) + R); + Vector r = y - (C * _x); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro() } // lower baro trust - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_baroFault) { _baroFault = FAULT_NONE; @@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar() R(0, 0) = cov; } - Matrix y; + Vector y; y.setZero(); y(0) = (d - _lidarAltHome) * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch); // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); - Matrix r = y - C * _x; + Matrix S_I = inv((C * _P * C.transpose()) + R); + Vector r = y - C * _x; // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); - Matrix y; + Vector y; y.setZero(); y(0) = px; y(1) = py; @@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met R(5, 5) = var_vz; // residual - Matrix r = y - C * _x; - Matrix S_I = (C * _P * C.transpose() + R).inverse(); + Vector r = y - C * _x; + Matrix S_I = inv(C * _P * C.transpose() + R); // fault detection float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); @@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met } // trust GPS less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_gpsFault) { _gpsFault = FAULT_NONE; @@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps() // TODO : use another other met void BlockLocalPositionEstimator::correctVision() { - Matrix y; + Vector y; y.setZero(); y(0) = _sub_vision_pos.get().x - _visionHome(0); y(1) = _sub_vision_pos.get().y - _visionHome(1); @@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision() R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix S_I = inv((C * _P * C.transpose()) + R); Matrix r = y - C * _x; // fault detection @@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision() } // trust less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_visionFault) { _visionFault = FAULT_NONE; @@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision() void BlockLocalPositionEstimator::correctmocap() { - Matrix y; + Vector y; y.setZero(); y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); @@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap() R(Y_mocap_z, Y_mocap_z) = mocap_p_var; // residual - Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix S_I = inv((C * _P * C.transpose()) + R); Matrix r = y - C * _x; // fault detection @@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap() } // trust less - S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + S_I = inv((C * _P * C.transpose()) + R * 10); } else if (_mocapFault) { _mocapFault = FAULT_NONE; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 66f301efe3..1c6c43fd20 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -6,7 +6,7 @@ #include #ifdef USE_MATRIX_LIB -#include "matrix/src/Matrix.hpp" +#include "matrix/Matrix.hpp" using namespace matrix; #else #include @@ -304,7 +304,7 @@ private: perf_counter_t _err_perf; // state space - Matrix _x; // state vector - Matrix _u; // input vector + Vector _x; // state vector + Vector _u; // input vector Matrix _P; // state covariance matrix }; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index e0d1be2d0a..761a0d94b0 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) elseif(${OS} STREQUAL "posix") list(APPEND MODULE_CFLAGS -Wno-error) - # add matrix tests - add_subdirectory(matrix) endif() # use custom matrix lib instead of Eigen diff --git a/src/modules/local_position_estimator/matrix/.gitignore b/src/modules/local_position_estimator/matrix/.gitignore deleted file mode 100644 index a5309e6b90..0000000000 --- a/src/modules/local_position_estimator/matrix/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build*/ diff --git a/src/modules/local_position_estimator/matrix/CMakeLists.txt b/src/modules/local_position_estimator/matrix/CMakeLists.txt deleted file mode 100644 index 5a16ced707..0000000000 --- a/src/modules/local_position_estimator/matrix/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 2.8) - -project(matrix CXX) - -set(CMAKE_BUILD_TYPE "RelWithDebInfo") - -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") - -enable_testing() - -include_directories(src) - -add_subdirectory(test) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp deleted file mode 100644 index ae1111894c..0000000000 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ /dev/null @@ -1,464 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace matrix -{ - -template -class Matrix -{ - -private: - T _data[M][N]; - size_t _rows; - size_t _cols; - -public: - - Matrix() : - _data(), - _rows(M), - _cols(N) - { - } - - Matrix(const T *data) : - _data(), - _rows(M), - _cols(N) - { - memcpy(_data, data, sizeof(_data)); - } - - Matrix(const Matrix &other) : - _data(), - _rows(M), - _cols(N) - { - memcpy(_data, other._data, sizeof(_data)); - } - - Matrix(T x, T y, T z) : - _data(), - _rows(M), - _cols(N) - { - // TODO, limit to only 3x1 matrices - _data[0][0] = x; - _data[1][0] = y; - _data[2][0] = z; - } - - /** - * Accessors/ Assignment etc. - */ - - T *data() - { - return _data[0]; - } - - inline size_t rows() const - { - return _rows; - } - - inline size_t cols() const - { - return _rows; - } - - inline T operator()(size_t i) const - { - return _data[i][0]; - } - - inline T operator()(size_t i, size_t j) const - { - return _data[i][j]; - } - - inline T &operator()(size_t i) - { - return _data[i][0]; - } - - inline T &operator()(size_t i, size_t j) - { - return _data[i][j]; - } - - /** - * Matrix Operations - */ - - // this might use a lot of programming memory - // since it instantiates a class for every - // required mult pair, but it provides - // compile time size_t checking - template - Matrix operator*(const Matrix &other) const - { - const Matrix &self = *this; - Matrix res; - res.setZero(); - - for (size_t i = 0; i < M; i++) { - for (size_t k = 0; k < P; k++) { - for (size_t j = 0; j < N; j++) { - res(i, k) += self(i, j) * other(j, k); - } - } - } - - return res; - } - - Matrix operator+(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) + other(i, j); - } - } - - return res; - } - - bool operator==(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - if (self(i , j) != other(i, j)) { - return false; - } - } - } - - return true; - } - - Matrix operator-(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) - other(i, j); - } - } - - return res; - } - - void operator+=(const Matrix &other) - { - Matrix &self = *this; - self = self + other; - } - - void operator-=(const Matrix &other) - { - Matrix &self = *this; - self = self - other; - } - - void operator*=(const Matrix &other) - { - Matrix &self = *this; - self = self * other; - } - - /** - * Scalar Operations - */ - - Matrix operator*(T scalar) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) * scalar; - } - } - - return res; - } - - Matrix operator+(T scalar) const - { - Matrix res; - Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(i , j) = self(i, j) + scalar; - } - } - - return res; - } - - void operator*=(T scalar) - { - Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - self(i, j) = self(i, j) * scalar; - } - } - } - - void operator/=(T scalar) - { - Matrix &self = *this; - self = self * (1.0f / scalar); - } - - /** - * Misc. Functions - */ - - void print() - { - Matrix &self = *this; - printf("\n"); - - for (size_t i = 0; i < M; i++) { - printf("["); - - for (size_t j = 0; j < N; j++) { - printf("%10g\t", double(self(i, j))); - } - - printf("]\n"); - } - } - - Matrix transpose() const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < N; j++) { - res(j, i) = self(i, j); - } - } - - return res; - } - - Matrix expm(float dt, size_t n) const - { - Matrix res; - res.setIdentity(); - Matrix A_pow = *this; - float k_fact = 1; - size_t k = 1; - - while (k < n) { - res += A_pow * (float(pow(dt, k)) / k_fact); - - if (k == n) { break; } - - A_pow *= A_pow; - k_fact *= k; - k++; - } - - return res; - } - - Matrix diagonal() const - { - Matrix res; - // force square for now - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - res(i) = self(i, i); - } - - return res; - } - - void setZero() - { - memset(_data, 0, sizeof(_data)); - } - - void setIdentity() - { - setZero(); - Matrix &self = *this; - - for (size_t i = 0; i < M and i < N; i++) { - self(i, i) = 1; - } - } - - inline void swapRows(size_t a, size_t b) - { - if (a == b) { return; } - - Matrix &self = *this; - - for (size_t j = 0; j < cols(); j++) { - T tmp = self(a, j); - self(a, j) = self(b, j); - self(b, j) = tmp; - } - } - - inline void swapCols(size_t a, size_t b) - { - if (a == b) { return; } - - Matrix &self = *this; - - for (size_t i = 0; i < rows(); i++) { - T tmp = self(i, a); - self(i, a) = self(i, b); - self(i, b) = tmp; - } - } - - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const - { - Matrix L; - L.setIdentity(); - const Matrix &A = (*this); - Matrix U = A; - Matrix P; - P.setIdentity(); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) { continue; } - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - Matrix zero; - zero.setZero(); - return zero; - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - -}; - -typedef Matrix Vector2f; -typedef Matrix Vector3f; -typedef Matrix Matrix3f; - -}; // namespace matrix diff --git a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt deleted file mode 100644 index cf280e287b..0000000000 --- a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -set(tests - setIdentity - inverse - matrixMult - vectorAssignment - matrixAssignment - matrixScalarMult - transpose - ) - -foreach(test ${tests}) - add_executable(${test} - ${test}.cpp) - add_test(${test} ${test}) -endforeach() diff --git a/src/modules/local_position_estimator/matrix/test/inverse.cpp b/src/modules/local_position_estimator/matrix/test/inverse.cpp deleted file mode 100644 index 73f7c0b432..0000000000 --- a/src/modules/local_position_estimator/matrix/test/inverse.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; - Matrix3f A(data); - Matrix3f A_I = A.inverse(); - float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; - Matrix3f A_I_check(data_check); - (void)A_I; - assert(A_I == A_I_check); - - // stess test - static const size_t n = 100; - Matrix A_large; - A_large.setIdentity(); - Matrix A_large_I; - A_large_I.setZero(); - - for (size_t i = 0; i < 100; i++) { - A_large_I = A_large.inverse(); - assert(A_large == A_large_I); - } - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp deleted file mode 100644 index 5a625d0cc1..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Matrix3f m; - m.setZero(); - m(0, 0) = 1; - m(0, 1) = 2; - m(0, 2) = 3; - m(1, 0) = 4; - m(1, 1) = 5; - m(1, 2) = 6; - m(2, 0) = 7; - m(2, 1) = 8; - m(2, 2) = 9; - - m.print(); - - float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; - Matrix3f m2(data); - m2.print(); - - assert(m == m2); - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp deleted file mode 100644 index 7b42d947d4..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; - Matrix3f A(data); - float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; - Matrix3f A_I(data_check); - Matrix3f I; - I.setIdentity(); - A.print(); - A_I.print(); - Matrix3f R = A * A_I; - R.print(); - assert(R == I); - - Matrix3f R2 = A; - R2 *= A_I; - R2.print(); - assert(R2 == I); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp deleted file mode 100644 index 78bdb5b27f..0000000000 --- a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; - Matrix3f A(data); - A = A * 2; - float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; - Matrix3f A_check(data_check); - A.print(); - A_check.print(); - assert(A == A_check); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp deleted file mode 100644 index f80e437939..0000000000 --- a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Matrix3f A; - A.setIdentity(); - assert(A.rows() == 3); - assert(A.cols() == 3); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - if (i == j) { - assert(A(i, j) == 1); - - } else { - assert(A(i, j) == 0); - } - } - } - - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/transpose.cpp b/src/modules/local_position_estimator/matrix/test/transpose.cpp deleted file mode 100644 index 3623fc1f9a..0000000000 --- a/src/modules/local_position_estimator/matrix/test/transpose.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "Matrix.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - float data[9] = {1, 2, 3, 4, 5, 6}; - Matrix A(data); - Matrix A_T = A.transpose(); - float data_check[9] = {1, 4, 2, 5, 3, 6}; - Matrix A_T_check(data_check); - A_T.print(); - A_T_check.print(); - assert(A_T == A_T_check); - return 0; -} diff --git a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp deleted file mode 100644 index 68f5070194..0000000000 --- a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "Matrix.hpp" -#include - -using namespace matrix; - -int main() -{ - Vector3f v; - v(0) = 1; - v(1) = 2; - v(2) = 3; - - v.print(); - - assert(v(0) == 1); - assert(v(1) == 2); - assert(v(2) == 3); - - Vector3f v2(4, 5, 6); - - v2.print(); - - assert(v2(0) == 4); - assert(v2(1) == 5); - assert(v2(2) == 6); - - return 0; -} diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index a863328f03..c8a0e7d870 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -56,7 +56,6 @@ px4_add_module( mavlink_rate_limiter.cpp mavlink_receiver.cpp mavlink_ftp.cpp - mavlink_params.c DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 4edaa7b582..e50a95a25f 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -31,8 +31,6 @@ * ****************************************************************************/ -#include - /** * MAVLink system ID * @group MAVLink 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 118a0709b8..477e364a5b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -74,7 +74,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: bool _task_should_exit; /**< if true, task_main() should exit */ int _control_task; /**< task handle */ - int _v_att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ int _v_control_mode_sub; /**< vehicle control mode subscription */ @@ -145,7 +145,7 @@ private: bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< control state */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ @@ -297,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _v_att_sub(-1), + _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), _params_sub(-1), @@ -319,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { - memset(&_v_att, 0, sizeof(_v_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); @@ -599,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt) math::Matrix<3, 3> R_sp; R_sp.set(_v_att_sp.R_body); - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.set(_v_att.R); + /* get current rotation matrix from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ @@ -693,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* current body angular rates */ math::Vector<3> rates; - rates(0) = _v_att.rollspeed; - rates(1) = _v_att.pitchspeed; - rates(2) = _v_att.yawspeed; + rates(0) = _ctrl_state.roll_rate; + rates(1) = _ctrl_state.pitch_rate; + rates(2) = _ctrl_state.yaw_rate; /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; @@ -733,7 +733,7 @@ MulticopterAttitudeControl::task_main() */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -747,7 +747,7 @@ MulticopterAttitudeControl::task_main() /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; - fds[0].fd = _v_att_sub; + fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -783,8 +783,8 @@ MulticopterAttitudeControl::task_main() dt = 0.02f; } - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + /* copy attitude and control state topics */ + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); @@ -862,7 +862,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _v_att.timestamp; + _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); diff --git a/src/modules/mc_att_control_multiplatform/CMakeLists.txt b/src/modules/mc_att_control_multiplatform/CMakeLists.txt index 22f9d16ee8..60aefe210a 100644 --- a/src/modules/mc_att_control_multiplatform/CMakeLists.txt +++ b/src/modules/mc_att_control_multiplatform/CMakeLists.txt @@ -38,7 +38,6 @@ px4_add_module( mc_att_control_start_nuttx.cpp mc_att_control.cpp mc_att_control_base.cpp - mc_att_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c index 9c6ac5dc73..e34732d1f3 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c @@ -36,14 +36,10 @@ * Parameters for multicopter attitude controller. * * @author Tobias Naegeli - * @author Lorenz Meier + * @author Lorenz Meier * @author Anton Babushkin */ -#include -#include "mc_att_control_params.h" -#include - /** * Roll P gain * 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 17bcb0f967..c36ba5bfc6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -119,14 +119,14 @@ public: int start(); private: - const float alt_ctl_dz = 0.2f; + const float alt_ctl_dz = 0.1f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ int _vehicle_status_sub; /**< vehicle status subscription */ - int _att_sub; /**< vehicle attitude subscription */ + int _ctrl_state_sub; /**< control state subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ @@ -142,7 +142,7 @@ private: orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct control_state_s _ctrl_state; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -230,6 +230,9 @@ private: math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; + math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ + float _yaw; /**< yaw angle (euler) */ + /** * Update our local parameter cache. */ @@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _mavlink_fd(-1), /* subscriptions */ - _att_sub(-1), + _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), @@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_hold_engaged(false), _alt_hold_engaged(false), _run_pos_control(true), - _run_alt_control(true) + _run_alt_control(true), + _yaw(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); - memset(&_att, 0, sizeof(_att)); + memset(&_ctrl_state, 0, sizeof(_ctrl_state)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_prev.zero(); _vel_ff.zero(); + _R.identity(); + _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.z_p = param_find("MPC_Z_P"); @@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); } - orb_check(_att_sub, &updated); + orb_check(_ctrl_state_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); } orb_check(_att_sp_sub, &updated); @@ -1009,7 +1015,7 @@ MulticopterPositionControl::task_main() * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -1064,6 +1070,14 @@ MulticopterPositionControl::task_main() } poll_subscriptions(); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); + parameters_update(false); hrt_abstime t = hrt_absolute_time(); @@ -1139,7 +1153,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -1324,11 +1338,11 @@ MulticopterPositionControl::task_main() /* thrust compensation for altitude only control mode */ float att_comp; - if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { - att_comp = 1.0f / PX4_R(_att.R, 2, 2); + if (_R(2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / _R(2, 2); - } else if (PX4_R(_att.R, 2, 2) > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; + } else if (_R(2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; saturation_z = true; } else { @@ -1515,7 +1529,7 @@ MulticopterPositionControl::task_main() /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + _att_sp.yaw_body = _yaw; } /* do not move yaw while arming */ @@ -1525,7 +1539,7 @@ MulticopterPositionControl::task_main() _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); - float yaw_offs = _wrap_pi(yaw_target - _att.yaw); + float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 7c2942f53e..1ef0c57e3e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -52,7 +52,7 @@ * @unit meters * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); /** * Enable persistent onboard mission storage diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1ddedddff4..6ecf73a606 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -135,7 +135,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } - bool home_position_valid() { return _home_position_set; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -205,8 +205,6 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; - bool _home_position_set; - bool _mission_item_valid; /**< flags if the current mission item is valid */ int _mission_instance_count; /**< instance count for the current mission */ @@ -257,7 +255,7 @@ private: /** * Retrieve home position */ - void home_position_update(); + void home_position_update(bool force=false); /** * Retreive navigation capabilities diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 92f3e2fecb..e44b8d0be5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,7 +126,6 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, - _home_position_set(false), _mission_item_valid(false), _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -206,17 +205,13 @@ Navigator::sensor_combined_update() } void -Navigator::home_position_update() +Navigator::home_position_update(bool force) { bool updated = false; orb_check(_home_pos_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - if (_home_pos.timestamp > 0) { - _home_position_set = true; - } } } @@ -298,7 +293,7 @@ Navigator::task_main() global_position_update(); gps_position_update(); sensor_combined_update(); - home_position_update(); + home_position_update(true); navigation_capabilities_update(); params_update(); @@ -408,7 +403,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; @@ -489,7 +484,7 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d68c90cf98..f15ec8969a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -155,7 +156,9 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char log_dir[32]; +#define LOG_BASE_PATH_LEN 64 + +static char log_dir[LOG_BASE_PATH_LEN]; /* statistics counters */ static uint64_t start_time = 0; @@ -422,7 +425,6 @@ int create_log_dir() mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { - warnx("log dir created: %s", log_dir); break; } else if (errno != EEXIST) { @@ -443,15 +445,15 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir); return 0; } int open_log_file() { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -479,7 +481,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -491,10 +493,10 @@ int open_log_file() #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name); } return fd; @@ -503,8 +505,8 @@ int open_log_file() int open_perf_file(const char* str) { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -531,7 +533,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -543,7 +545,7 @@ int open_perf_file(const char* str) #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } @@ -670,7 +672,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir"); return; } @@ -760,7 +762,7 @@ void sdlog2_stop_log() /* free log writer performance counter */ perf_free(perf_write); - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped"); sdlog2_status(); } @@ -1089,6 +1091,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vtol_vehicle_status_s vtol_status; struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; + struct control_state_s ctrl_state; } buf; memset(&buf, 0, sizeof(buf)); @@ -1137,6 +1140,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ENCD_s log_ENCD; struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; + struct log_CTS_s log_CTS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1179,6 +1183,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int encoders_sub; int tsync_sub; int mc_att_ctrl_status_sub; + int ctrl_state_sub; } subs; subs.cmd_sub = -1; @@ -1212,6 +1217,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = -1; subs.tsync_sub = -1; subs.mc_att_ctrl_status_sub = -1; + subs.ctrl_state_sub = -1; subs.encoders_sub = -1; /* add new topics HERE */ @@ -1670,6 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); @@ -1856,6 +1863,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(MACS); } + /* --- CONTROL STATE --- */ + if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) { + log_msg.msg_type = LOG_CTS_MSG; + log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel; + log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel; + log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel; + log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed; + log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate; + log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate; + log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate; + LOGBUFFER_WRITE_AND_COUNT(CTS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ @@ -1893,7 +1913,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } @@ -1918,7 +1938,7 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] no space on MicroSD: %u MiB", + "[log] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; @@ -1926,7 +1946,7 @@ int check_free_space() /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] space on MicroSD low: %u MiB", + "[log] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1f1b979288..6ccbec2fa9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -189,6 +189,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; }; @@ -477,6 +478,18 @@ struct log_MACS_s { float yaw_rate_integ; }; +/* --- CONTROL STATE --- */ +#define LOG_CTS_MSG 47 +struct log_CTS_s { + float vx_body; + float vy_body; + float vz_body; + float airspeed; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + /* WARNING: ID 46 is already in use for ATTC1 */ /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -519,7 +532,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), - LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), + LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), + LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e330ca7a2..ab246e14a1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -850,7 +850,7 @@ Sensors::parameters_update() _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V1 _parameters.battery_voltage_scaling = 0.00459340659f; #else /* ensure a missing default trips a low voltage lockdown */ @@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll() void Sensors::parameter_update_poll(bool forced) { - bool param_updated; + bool param_updated = false; /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); @@ -2146,7 +2146,7 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; /* use the gyro to pace output */ fds[0].fd = _gyro_sub[0]; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index f99dd20450..ca45cdf2b8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish) char serial_buf[1024]; struct pollfd fds[2]; + memset(fds, 0, sizeof(fds)); unsigned fd_count = 1; fds[0].fd = _fd; fds[0].events = POLLIN; + if (serial_fd >= 0) { fds[1].fd = serial_fd; fds[1].events = POLLIN; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c76e6c37f3..b4d9b5d1c6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -56,6 +56,7 @@ #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 #define CBRK_GPSFAIL_KEY 240024 +#define CBRK_USB_CHK_KEY 197848 #include diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 003b73bb62..a3eaebbb33 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_BUZZER, 0); + +/** + * Circuit breaker for USB link check + * + * Setting this parameter to 197848 will disable the USB connected + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 197848 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_USB_CHK, 0); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 25dea7a9b0..622a3a0603 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -52,7 +52,7 @@ #define RC_INPUT_MAP_UNMAPPED 0 -int rc_calibration_check(int mavlink_fd) +int rc_calibration_check(int mavlink_fd, bool report_fail) { char nbuf[20]; @@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } if (mapping == 0) { - mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd) /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); + + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -187,8 +199,13 @@ int rc_calibration_check(int mavlink_fd) if (channels_failed) { sleep(2); - mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + } + usleep(100000); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 035f63bef4..239c8a6a13 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(int mavlink_fd); +__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail); __END_DECLS diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 8170e17bd1..0bf1a3ea9e 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,7 +40,7 @@ /** * Auto-start script index. * - * Defines the auto-start script used to bootstrap the system. + * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. * * @group System */ @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); /** * Companion computer interface * -* Configures the baud rate of the companion computer interface. +* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface. * Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) * 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. * 157600: enables OSD mode at 57600 baud, 8N1. diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ac98048771..8fe70d7604 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s); #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); +#include "topics/control_state.h" +ORB_DEFINE(control_state, struct control_state_s); + #include "topics/sensor_combined.h" ORB_DEFINE(sensor_combined, struct sensor_combined_s); diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index b8c899840a..ce348ec910 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") string(TOUPPER "${OS}" OS_UPPER) add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=2 -DUAVCAN_STM32_TIMER_NUMBER=5 - -DUAVCAN_USE_CPP03=ON - -DUAVCAN_USE_EXTERNAL_SNPRINT ) add_subdirectory(libuavcan EXCLUDE_FROM_ALL) diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp new file mode 100644 index 0000000000..db34aa04fb --- /dev/null +++ b/src/modules/uavcan/allocator.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step. +namespace uavcan_node +{ + +struct AllocatorSynchronizer +{ + const ::irqstate_t state = ::irqsave(); + ~AllocatorSynchronizer() { ::irqrestore(state); } +}; + +struct Allocator : public uavcan::HeapBasedPoolAllocator +{ + static constexpr unsigned CapacitySoftLimit = 250; + static constexpr unsigned CapacityHardLimit = 500; + + Allocator() : + uavcan::HeapBasedPoolAllocator(CapacitySoftLimit, CapacityHardLimit) + { } + + ~Allocator() + { + if (getNumAllocatedBlocks() > 0) + { + warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", + getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); + } + } +}; + +} diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 0643879922..ed1d71e639 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 0643879922239930cf7482777356f06891c26616 +Subproject commit ed1d71e639543ea5743a839c313c53237ab3a27d diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 694a3988a1..36f28a423c 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node), - _reports(nullptr) + _reports(2, sizeof(baro_report)) { } int UavcanBarometerBridge::init() @@ -55,13 +55,6 @@ int UavcanBarometerBridge::init() return res; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - - if (_reports == nullptr) { - return -1; - } - res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { @@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl } while (count--) { - if (_reports->get(baro_buf)) { + if (_reports.get(baro_buf)) { ret += sizeof(*baro_buf); baro_buf++; } @@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + if (!_reports.resize(arg)) { irqrestore(flags); return -ENOMEM; } @@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; // add to the ring buffer - _reports->force(&report); + _reports.force(&report); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 38cf6ab89a..ed1e308515 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -76,8 +76,10 @@ private: uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; + + ringbuffer::RingBuffer _reports; + unsigned _msl_pressure = 101325; - ringbuffer::RingBuffer *_reports; float last_temperature_kelvin = 0.0f; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 1ba9d345fb..00671decd2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,7 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _node(can_driver, system_clock), + _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), _time_sync_master(_node), @@ -144,10 +144,9 @@ UavcanNode::~UavcanNode() } while (_task != -1); } - /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - ::close(_armed_sub); + (void)::close(_armed_sub); + (void)::close(_test_motor_sub); + (void)::close(_actuator_direct_sub); // Removing the sensor bridges auto br = _sensor_bridges.getHead(); @@ -166,6 +165,10 @@ UavcanNode::~UavcanNode() pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); + // Is it allowed to delete it like that? + if (_mixers != nullptr) { + delete _mixers; + } } int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) @@ -965,6 +968,8 @@ int UavcanNode::run() } } + (void)::close(busevent_fd); + teardown(); warnx("exiting."); @@ -1119,6 +1124,33 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); + // Memory status + printf("Pool allocator status:\n"); + printf("\tCapacity hard/soft: %u/%u blocks\n", + _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); + printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); + printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + + // UAVCAN node perfcounters + printf("UAVCAN node status:\n"); + printf("\tInternal failures: %llu\n", _node.getInternalFailureCount()); + printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount()); + printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount()); + printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + + // CAN driver status + for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + printf("CAN%u status:\n", unsigned(i + 1)); + + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + printf("\tHW errors: %llu\n", iface->getErrorCount()); + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + printf("\tIO errors: %llu\n", iface_perf_cnt.errors); + printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx); + printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); + } + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); @@ -1169,13 +1201,20 @@ UavcanNode::print_info() (void)pthread_mutex_unlock(&_node_mutex); } +void UavcanNode::shrink() +{ + (void)pthread_mutex_lock(&_node_mutex); + _pool_allocator.shrink(); + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1251,6 +1290,11 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "shrink")) { + inst->shrink(); + ::exit(0); + } + if (!std::strcmp(argv[1], "arm")) { inst->arm_actuators(true); ::exit(0); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 6a1d2f391d..b236946e24 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -55,6 +56,7 @@ #include "sensors/sensor_bridge.hpp" #include "uavcan_servers.hpp" +#include "allocator.hpp" /** * @file uavcan_main.hpp @@ -78,9 +80,8 @@ class UavcanNode : public device::CDev static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned PollTimeoutMs = 3; - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -97,7 +98,6 @@ class UavcanNode : public device::CDev static constexpr unsigned StackSize = 1800; public: - typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -109,7 +109,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node &get_node() { return _node; } + uavcan::Node<> &get_node() { return _node; } // TODO: move the actuator mixing stuff into the ESC controller class static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); @@ -121,6 +121,8 @@ public: void print_info(); + void shrink(); + static UavcanNode *instance() { return _instance; } static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); @@ -130,8 +132,8 @@ public: int set_param(int remote_node_id, const char *name, char *value); int get_param(int remote_node_id, const char *name); int reset_node(int remote_node_id); -private: +private: void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -167,7 +169,9 @@ private: static UavcanNode *_instance; ///< singleton pointer - Node _node; ///< library instance + uavcan_node::Allocator _pool_allocator; + + uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 713a4e00f7..ca5bcede66 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -85,8 +85,8 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), - _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()), _main_node(main_node), _tracer(), _storage_backend(), @@ -141,13 +141,14 @@ int UavcanServers::stop() return -1; } - _instance = nullptr; - - if (server->_subnode_thread != -1) { - pthread_cancel(server->_subnode_thread); - pthread_join(server->_subnode_thread, NULL); + if (server->_subnode_thread) { + warnx("stopping fw srv thread..."); + server->_subnode_thread_should_exit = true; + (void)pthread_join(server->_subnode_thread, NULL); } + _instance = nullptr; + server->_main_node.getDispatcher().removeRxFrameListener(); delete server; @@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; - while (1) { + while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; @@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } } - warnx("exiting."); + _subnode_thread_should_exit = false; + + warnx("exiting"); return (pthread_addr_t) 0; } diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 0b83baa941..3cf4cbb4da 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -81,24 +81,10 @@ class UavcanServers { static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - - static constexpr unsigned MaxCanFramesPerTransfer = 63; - - /** - * This number is based on the worst case max number of frames per interface. With - * MemPoolBlockSize set at 48 this is 6048 Bytes. - * - * The servers can be forced to use the primary interface only, this can be achieved simply by passing - * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. - */ - static constexpr unsigned QueuePoolSize = - (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; - typedef uavcan::SubNode SubNode; + static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80; public: UavcanServers(uavcan::INode &main_node); @@ -108,7 +94,7 @@ public: static int start(uavcan::INode &main_node); static int stop(); - SubNode &get_node() { return _subnode; } + uavcan::SubNode<> &get_node() { return _subnode; } static UavcanServers *instance() { return _instance; } @@ -124,6 +110,7 @@ public: private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; + volatile bool _subnode_thread_should_exit = false; int init(); @@ -131,12 +118,10 @@ private: static UavcanServers *_instance; ///< singleton pointer - typedef VirtualCanDriver vCanDriver; + VirtualCanDriver _vdriver; - vCanDriver _vdriver; - - uavcan::SubNode _subnode; ///< library instance - uavcan::INode &_main_node; ///< library instance + uavcan::SubNode<> _subnode; + uavcan::INode &_main_node; uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 1c861e481c..956caf11ee 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -114,6 +114,14 @@ public: uavcan::IsDynamicallyAllocatable::check(); } + ~Queue() + { + while (!isEmpty()) + { + pop(); + } + } + bool isEmpty() const { return list_.isEmpty(); } /** @@ -329,11 +337,7 @@ public: /** * Objects of this class are owned by the sub-node thread. * This class does not use heap memory. - * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the - * memory pool that will be shared across all interfaces for RX/TX queues. - * Typically this value should be no less than 4K per interface. */ -template class VirtualCanDriver : public uavcan::ICanDriver, public uavcan::IRxFrameListener, public ITxQueueInjector, @@ -400,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver, } }; - Event event_; ///< Used to unblock the select() call when IO happens. + Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::PoolAllocator allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -476,7 +479,10 @@ class VirtualCanDriver : public uavcan::ICanDriver, } public: - VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + VirtualCanDriver(unsigned arg_num_ifaces, + uavcan::ISystemClock &system_clock, + uavcan::IPoolAllocator& allocator, + unsigned virtual_iface_block_allocation_quota) : num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -485,15 +491,12 @@ public: assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); - const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; - const unsigned quota_per_queue = quota_per_iface; // 2x overcommit - - UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", - unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].template + construct + (allocator, clock_, driver_mutex_, quota_per_queue); } } diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index 6af889f52a..b7dcd83ed0 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -38,12 +38,9 @@ px4_add_module( SRCS vtol_att_control_main.cpp - vtol_att_control_params.c - tiltrotor_params.c tiltrotor.cpp vtol_type.cpp tailsitter.cpp - standard_params.c standard.cpp DEPENDS platforms__common diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 649c4b526f..05c1a525b1 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); - } +} Standard::~Standard() { @@ -98,12 +98,12 @@ Standard::parameters_update() void Standard::update_vtol_state() { - parameters_update(); + parameters_update(); /* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up - * forward speed. After the vehicle has picked up enough speed the rotors shutdown. + * forward speed. After the vehicle has picked up enough speed the rotors shutdown. * For the back transition the pusher motor is immediately stopped and rotors reactivated. - */ + */ if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off @@ -130,7 +130,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed if (hrt_elapsed_time(&_vtol_schedule.transition_start) > - (_params_standard.back_trans_dur * 1000000.0f)) { + (_params_standard.back_trans_dur * 1000000.0f)) { _vtol_schedule.flight_mode = MC_MODE; } } @@ -154,7 +154,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; @@ -168,17 +168,19 @@ void Standard::update_vtol_state() } // map specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - break; - case TRANSITION_TO_FW: - case TRANSITION_TO_MC: - _vtol_mode = TRANSITION; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_TO_FW: + case TRANSITION_TO_MC: + _vtol_mode = TRANSITION; + break; } } @@ -188,18 +190,21 @@ void Standard::update_transition_state() if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value _pusher_throttle = _params_standard.pusher_trans; + } else if (_pusher_throttle <= _params_standard.pusher_trans) { // ramp up throttle to the target throttle value - _pusher_throttle = _params_standard.pusher_trans * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + _pusher_throttle = _params_standard.pusher_trans * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls if a blending airspeed has been provided if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { - float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / + _airspeed_trans_blend_margin; _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -210,17 +215,19 @@ void Standard::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { - float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * + 1000000.0f); _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + } else { _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; } - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); @@ -238,15 +245,15 @@ void Standard::update_mc_state() // do nothing } - void Standard::update_fw_state() +void Standard::update_fw_state() { - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle _flag_enable_mc_motors = true; } - } +} void Standard::update_external_state() { @@ -259,22 +266,31 @@ void Standard::update_external_state() void Standard::fill_actuator_outputs() { /* multirotor controls */ - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] + * _mc_roll_weight; // roll + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * + _mc_yaw_weight; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle /* fixed wing controls */ - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); //roll + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control if (_vtol_schedule.flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { - // otherwise we may be ramping up the throttle during the transition to fw mode + // otherwise we may be ramping up the throttle during the transition to fw mode _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; } } @@ -288,11 +304,11 @@ Standard::set_max_mc(unsigned pwm_value) int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); @@ -301,9 +317,9 @@ Standard::set_max_mc(unsigned pwm_value) pwm_values.channel_count = _params->vtol_motor_count; } - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting max values");} + if (ret != OK) {PX4_WARN("failed setting max values");} - close(fd); + px4_close(fd); } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 7bce82071a..ed37ccc398 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -31,15 +31,15 @@ * ****************************************************************************/ - /** - * @file standard.h - * VTOL with fixed multirotor motor configurations (such as quad) and a pusher - * (or puller aka tractor) motor for forward flight. - * - * @author Simon Wilks - * @author Roman Bapst - * - */ +/** +* @file standard.h +* VTOL with fixed multirotor motor configurations (such as quad) and a pusher +* (or puller aka tractor) motor for forward flight. +* +* @author Simon Wilks +* @author Roman Bapst +* +*/ #ifndef STANDARD_H #define STANDARD_H @@ -52,7 +52,7 @@ class Standard : public VtolType public: - Standard(VtolAttitudeControl * _att_controller); + Standard(VtolAttitudeControl *_att_controller); ~Standard(); void update_vtol_state(); @@ -89,7 +89,7 @@ private: struct { vtol_mode flight_mode; // indicates in which mode the vehicle is in hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) - }_vtol_schedule; + } _vtol_schedule; bool _flag_enable_mc_motors; float _pusher_throttle; diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 134dcd0b8a..3a2185eead 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -36,11 +36,9 @@ * Parameters for the standard VTOL controller. * * @author Simon Wilks - * @author Roman Bapst + * @author Roman Bapst */ -#include - /** * Target throttle value for pusher/puller motor during the transition to fw mode * diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 93a0f25f56..7368870847 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,21 +31,21 @@ * ****************************************************************************/ - /** - * @file tailsitter.cpp - * - * @author Roman Bapst - * - */ +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* +*/ - #include "tailsitter.h" - #include "vtol_att_control_main.h" +#include "tailsitter.h" +#include "vtol_att_control_main.h" -Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : -VtolType(att_controller), -_airspeed_tot(0), -_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), -_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) : + VtolType(att_controller), + _airspeed_tot(0), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { } @@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state() // simply switch between the two modes if (!_attc->is_fixed_wing_requested()) { _vtol_mode = ROTARY_WING; + } else { _vtol_mode = FIXED_WING; } @@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_mc_state() { - if (!flag_idle_mc) { + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } @@ -91,18 +92,18 @@ void Tailsitter::update_external_state() } - void Tailsitter::calc_tot_airspeed() - { +void Tailsitter::calc_tot_airspeed() +{ float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama // calculate momentary power of one engine float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; - P = math::constrain(P,1.0f,_params->power_max); + P = math::constrain(P, 1.0f, _params->power_max); // calculate prop efficiency - float power_factor = 1.0f - P*_params->prop_eff/_params->power_max; - float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; - eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + float power_factor = 1.0f - P * _params->prop_eff / _params->power_max; + float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f; + eta = math::constrain(eta, 0.001f, 1.0f); // live on the safe side // calculate induced airspeed by propeller - float v_ind = (airspeed/eta - airspeed)*2.0f; + float v_ind = (airspeed / eta - airspeed) * 2.0f; // calculate total airspeed float airspeed_raw = airspeed + v_ind; // apply low-pass filter @@ -115,16 +116,19 @@ Tailsitter::scale_mc_output() // scale around tuning airspeed float airspeed; calc_tot_airspeed(); // estimate air velocity seen by elevons + // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { airspeed = _params->mc_airspeed_trim; + if (nonfinite) { perf_count(_nonfinite_input_perf); } + } else { airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max); + airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max); } _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging @@ -135,8 +139,10 @@ Tailsitter::scale_mc_output() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed); - _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : + airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling, + -1.0f, 1.0f); } /** @@ -144,37 +150,50 @@ Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - 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]; + 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 == 1) { - _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; - 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; - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; - _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] + _params->fw_pitch_trim; // 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: - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + } 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; + + 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; + _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] + _params->fw_pitch_trim; // 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: + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 388111ace8..ec8d007567 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ #ifndef TAILSITTER_H #define TAILSITTER_H @@ -48,7 +48,7 @@ class Tailsitter : public VtolType { public: - Tailsitter(VtolAttitudeControl * _att_controller); + Tailsitter(VtolAttitudeControl *_att_controller); ~Tailsitter(); void update_vtol_state(); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index a50079b14b..270aa065a9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -44,12 +44,10 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : -VtolType(attc), -_rear_motors(ENABLED), -_tilt_control(0.0f), -_roll_weight_mc(1.0f), -_yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f) + VtolType(attc), + _rear_motors(ENABLED), + _tilt_control(0.0f), + _min_front_trans_dur(0.5f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -88,11 +86,11 @@ Tiltrotor::parameters_update() /* vtol duration of a front transition */ param_get(_params_handles_tiltrotor.front_trans_dur, &v); - _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + _params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f); /* vtol duration of a back transition */ param_get(_params_handles_tiltrotor.back_trans_dur, &v); - _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + _params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f); /* vtol tilt mechanism position in mc mode */ param_get(_params_handles_tiltrotor.tilt_mc, &v); @@ -125,22 +123,26 @@ Tiltrotor::parameters_update() /* avoid parameters which will lead to zero division in the transition code */ _params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur); - if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) { + if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) { _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; } return OK; } -int Tiltrotor::get_motor_off_channels(int channels) { +int Tiltrotor::get_motor_off_channels(int channels) +{ int channel_bitmap = 0; - + int channel; + for (int i = 0; i < _params->vtol_motor_count; ++i) { channel = channels % 10; + if (channel == 0) { break; } + channel_bitmap |= 1 << channel; channels = channels / 10; } @@ -150,82 +152,99 @@ int Tiltrotor::get_motor_off_channels(int channels) { void Tiltrotor::update_vtol_state() { - parameters_update(); + parameters_update(); - /* simple logic using a two way switch to perform transitions. + /* 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 * forward completely. For the backtransition the motors simply rotate back. - */ + */ if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case TRANSITION_FRONT_P1: - // failsafe into multicopter mode + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + if (_tilt_control <= _params_tiltrotor.tilt_mc) { _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_FRONT_P2: - // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_BACK: - if (_tilt_control <= _params_tiltrotor.tilt_mc) { - _vtol_schedule.flight_mode = MC_MODE; - } - break; + } + + break; } + } else { - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + // initialise a front transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed to switch to fw mode + // also allow switch if we are not armed for the sake of bench testing + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case FW_MODE: - break; - case TRANSITION_FRONT_P1: - // check if we have reached airspeed to switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); - } - break; - case TRANSITION_FRONT_P2: - // if the rotors have been tilted completely we switch to fw mode - if (_tilt_control >= _params_tiltrotor.tilt_fw) { - _vtol_schedule.flight_mode = FW_MODE; - _tilt_control = _params_tiltrotor.tilt_fw; - } - break; - case TRANSITION_BACK: - // failsafe into fixed wing mode + } + + break; + + case TRANSITION_FRONT_P2: + + // if the rotors have been tilted completely we switch to fw mode + if (_tilt_control >= _params_tiltrotor.tilt_fw) { _vtol_schedule.flight_mode = FW_MODE; - break; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + break; } } // map tiltrotor specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - break; - case TRANSITION_FRONT_P1: - case TRANSITION_FRONT_P2: - case TRANSITION_BACK: - _vtol_mode = TRANSITION; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + + case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P2: + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + break; } } @@ -250,7 +269,7 @@ void Tiltrotor::update_mc_state() _mc_yaw_weight = 1.0f; } - void Tiltrotor::update_fw_state() +void Tiltrotor::update_fw_state() { // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; @@ -278,15 +297,18 @@ void Tiltrotor::update_transition_state() if (_rear_motors != ENABLED) { set_rear_motor_state(ENABLED); } + // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition ) { + if (_tilt_control <= _params_tiltrotor.tilt_transition) { _tilt_control = _params_tiltrotor.tilt_mc + - fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur * 1000000.0f); + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { _mc_roll_weight = 0.0f; + } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; @@ -294,6 +316,7 @@ void Tiltrotor::update_transition_state() // disable mc yaw control once the plane has picked up speed _mc_yaw_weight = 1.0f; + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; } @@ -301,8 +324,10 @@ void Tiltrotor::update_transition_state() } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); _mc_roll_weight = 0.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_rear_motors != IDLE) { set_rear_motor_state(IDLE); @@ -312,10 +337,12 @@ void Tiltrotor::update_transition_state() set_idle_mc(); flag_idle_mc = true; } + // tilt rotors back if (_tilt_control > _params_tiltrotor.tilt_mc) { _tilt_control = _params_tiltrotor.tilt_fw - - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f); + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); } // set zero throttle for backtransition otherwise unwanted moments will be created @@ -339,19 +366,28 @@ void Tiltrotor::update_external_state() */ void Tiltrotor::fill_actuator_outputs() { - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_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; 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_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + } else { - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; } - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] + * (1 - _mc_yaw_weight); // yaw _actuators_out_1->control[4] = _tilt_control; } @@ -360,52 +396,58 @@ void Tiltrotor::fill_actuator_outputs() * Set state of rear motors. */ -void Tiltrotor::set_rear_motor_state(rear_motor_state state) { +void Tiltrotor::set_rear_motor_state(rear_motor_state state) +{ int pwm_value = PWM_DEFAULT_MAX; // map desired rear rotor state to max allowed pwm signal switch (state) { - case ENABLED: - pwm_value = PWM_DEFAULT_MAX; - _rear_motors = ENABLED; - break; - case DISABLED: - pwm_value = PWM_LOWEST_MAX; - _rear_motors = DISABLED; - break; - case IDLE: - pwm_value = _params->idle_pwm_mc; - _rear_motors = IDLE; - break; + case ENABLED: + pwm_value = PWM_DEFAULT_MAX; + _rear_motors = ENABLED; + break; + + case DISABLED: + pwm_value = PWM_LOWEST_MAX; + _rear_motors = DISABLED; + break; + + case IDLE: + pwm_value = _params->idle_pwm_mc; + _rear_motors = IDLE; + break; } int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; + } else { pwm_values.values[i] = PWM_DEFAULT_MAX; } + pwm_values.channel_count = _params->vtol_motor_count; } - ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting max values");} + if (ret != OK) {PX4_WARN("failed setting max values");} - close(fd); + px4_close(fd); } -bool Tiltrotor::is_motor_off_channel(const int channel) { +bool Tiltrotor::is_motor_off_channel(const int channel) +{ return (_params_tiltrotor.fw_motors_off >> channel) & 1; } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 859731266a..507920cb42 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* +*/ #ifndef TILTROTOR_H #define TILTROTOR_H @@ -49,7 +49,7 @@ class Tiltrotor : public VtolType public: - Tiltrotor(VtolAttitudeControl * _att_controller); + Tiltrotor(VtolAttitudeControl *_att_controller); ~Tiltrotor(); /** @@ -127,11 +127,9 @@ private: struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ hrt_abstime transition_start; /**< absoulte time at which front transition started */ - }_vtol_schedule; + } _vtol_schedule; float _tilt_control; /**< actuator value for the tilt servo */ - float _roll_weight_mc; /**< multicopter desired roll moment weight */ - float _yaw_weight_mc; /**< multicopter desired yaw moment weight */ const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d1b5b31081..58062c40b0 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -35,7 +35,7 @@ * @file tiltrotor_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst + * @author Roman Bapst */ #include 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 744f31d5b4..0248a20727 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -93,10 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); - memset(&_local_pos,0,sizeof(_local_pos)); - memset(&_airspeed,0,sizeof(_airspeed)); - memset(&_batt_status,0,sizeof(_batt_status)); - memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_airspeed, 0, sizeof(_airspeed)); + memset(&_batt_status, 0, sizeof(_batt_status)); + memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() : if (_params.vtol_type == 0) { _tailsitter = new Tailsitter(this); _vtol_type = _tailsitter; + } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { _standard = new Standard(this); _vtol_type = _standard; + } else { _task_should_exit = true; } @@ -150,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll() * Check for airspeed updates. */ void -VtolAttitudeControl::vehicle_airspeed_poll() { +VtolAttitudeControl::vehicle_airspeed_poll() +{ bool updated; orb_check(_airspeed_sub, &updated); @@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() { * Check for battery updates. */ void -VtolAttitudeControl::vehicle_battery_poll() { +VtolAttitudeControl::vehicle_battery_poll() +{ bool updated; orb_check(_battery_status_sub, &updated); @@ -442,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) void VtolAttitudeControl::task_main() { - warnx("started"); + PX4_WARN("started"); fflush(stdout); /* do subscriptions */ @@ -471,7 +476,7 @@ void VtolAttitudeControl::task_main() _vtol_type->set_idle_mc(); /* wakeup source*/ - struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; @@ -491,7 +496,7 @@ void VtolAttitudeControl::task_main() } /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ @@ -633,7 +638,7 @@ void VtolAttitudeControl::task_main() warnx("exit"); _control_task = -1; - _exit(0); + return; } int @@ -643,14 +648,14 @@ VtolAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - (main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (px4_main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { - warn("task start failed"); + PX4_WARN("task start failed"); return -errno; } @@ -661,49 +666,55 @@ VtolAttitudeControl::start() int vtol_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: vtol_att_control {start|stop|status}"); + PX4_WARN("usage: vtol_att_control {start|stop|status}"); } if (!strcmp(argv[1], "start")) { if (VTOL_att_control::g_control != nullptr) { - errx(1, "already running"); + PX4_WARN("already running"); + return 0; } VTOL_att_control::g_control = new VtolAttitudeControl; if (VTOL_att_control::g_control == nullptr) { - errx(1, "alloc failed"); + PX4_WARN("alloc failed"); + return 1; } if (OK != VTOL_att_control::g_control->start()) { delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - err(1, "start failed"); + PX4_WARN("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (VTOL_att_control::g_control == nullptr) { - errx(1, "not running"); + PX4_WARN("not running"); + return 0; } delete VTOL_att_control::g_control; VTOL_att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (VTOL_att_control::g_control) { - errx(0, "running"); + PX4_WARN("running"); } else { - errx(1, "not running"); + PX4_WARN("not running"); } + + return 0; } - warnx("unrecognized command"); + PX4_WARN("unrecognized command"); return 1; } 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 c0777fdeeb..c10f7b0dd7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -46,6 +46,10 @@ #ifndef VTOL_ATT_CONTROL_MAIN_H #define VTOL_ATT_CONTROL_MAIN_H +#include +#include +#include +#include #include #include #include @@ -80,7 +84,6 @@ #include #include #include -#include #include #include "tiltrotor.h" @@ -101,24 +104,24 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s* get_att () {return &_v_att;} - struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;} - struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;} - struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;} - struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;} - struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;} - struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;} - struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;} - struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;} - struct actuator_armed_s* get_armed () {return &_armed;} - struct vehicle_local_position_s* get_local_pos () {return &_local_pos;} - struct airspeed_s* get_airspeed () {return &_airspeed;} - struct battery_status_s* get_batt_status () {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} - struct Params* get_params () {return &_params;} + struct Params *get_params() {return &_params;} private: @@ -184,17 +187,12 @@ private: param_t elevons_mc_lock; } _params_handles; - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - unsigned _motor_count; // number of motors - float _airspeed_tot; int _transition_command; - VtolType * _vtol_type; // base class for different vtol types - Tiltrotor * _tiltrotor; // tailsitter vtol type - Tailsitter * _tailsitter; // tiltrotor vtol type - Standard * _standard; // standard vtol type + VtolType *_vtol_type; // base class for different vtol types + Tiltrotor *_tiltrotor; // tailsitter vtol type + Tailsitter *_tailsitter; // tiltrotor vtol type + Standard *_standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 56779ca8f9..fe7d8ebd82 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -35,11 +35,9 @@ * @file vtol_att_control_params.c * Parameters for vtol attitude controller. * - * @author Roman Bapst + * @author Roman Bapst */ -#include - /** * VTOL number of engines * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index c6917e3ced..f57080a2a7 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -31,21 +31,21 @@ * ****************************************************************************/ - /** - * @file airframe.cpp - * - * @author Roman Bapst - * - */ +/** +* @file airframe.cpp +* +* @author Roman Bapst +* +*/ #include "vtol_type.h" #include "drivers/drv_pwm_output.h" -#include +#include #include "vtol_att_control_main.h" VtolType::VtolType(VtolAttitudeControl *att_controller) : -_attc(att_controller), -_vtol_mode(ROTARY_WING) + _attc(att_controller), + _vtol_mode(ROTARY_WING) { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); @@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING) VtolType::~VtolType() { - + } /** @@ -81,11 +81,11 @@ void VtolType::set_idle_mc() int ret; unsigned servo_count; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); unsigned pwm_value = _params->idle_pwm_mc; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); @@ -95,11 +95,11 @@ void VtolType::set_idle_mc() pwm_values.channel_count++; } - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting min values");} + if (ret != OK) {PX4_WARN("failed setting min values");} - close(fd); + px4_close(fd); flag_idle_mc = true; } @@ -111,9 +111,9 @@ void VtolType::set_idle_fw() { int ret; char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); + int fd = px4_open(dev, 0); - if (fd < 0) {err(1, "can't open %s", dev);} + if (fd < 0) {PX4_WARN("can't open %s", dev);} unsigned pwm_value = PWM_LOWEST_MIN; struct pwm_output_values pwm_values; @@ -125,9 +125,9 @@ void VtolType::set_idle_fw() pwm_values.channel_count++; } - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - if (ret != OK) {errx(ret, "failed setting min values");} + if (ret != OK) {PX4_WARN("failed setting min values");} - close(fd); + px4_close(fd); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index a797201e01..d8557110c0 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -31,12 +31,12 @@ * ****************************************************************************/ - /** - * @file airframe.h - * - * @author Roman Bapst - * - */ +/** +* @file airframe.h +* +* @author Roman Bapst +* +*/ #ifndef VTOL_TYPE_H #define VTOL_TYPE_H @@ -85,7 +85,7 @@ public: void set_idle_mc(); void set_idle_fw(); - mode get_mode () {return _vtol_mode;}; + mode get_mode() {return _vtol_mode;}; protected: VtolAttitudeControl *_attc; diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 09defae9b4..f7485a692c 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -35,6 +35,7 @@ set(depends prebuild_targets git_mavlink git_uavcan + git_matrix ) px4_add_module( diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index d9f0dd4f15..c72cc1a494 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -111,6 +111,7 @@ private: ADCSIM::ADCSIM(uint32_t channels) : VDev("adcsim", ADCSIM0_DEVICE_PATH), + _call(), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0544131705..e4839f0ac7 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -81,35 +81,23 @@ static void hrt_unlock(void) px4_sem_post(&_hrt_lock); } -#ifdef __PX4_DARWIN - -#include -#define MAC_NANO (+1.0E-9) -#define MAC_GIGA UINT64_C(1000000000) -#define CLOCK_MONOTONIC 1 -#define HRT_LOCK_NAME "/hrt_lock" - -static double px4_timebase = 0.0; +#if defined(__APPLE__) && defined(__MACH__) +#include +#include +#define CLOCK_REALTIME 0 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) { - if (clk_id != CLOCK_MONOTONIC) { - return 1; + struct timeval now; + int rv = gettimeofday(&now, NULL); + + if (rv) { + return rv; } - if (!px4_timestart) { - mach_timebase_info_data_t tb = {}; - mach_timebase_info(&tb); - px4_timebase = tb.numer; - px4_timebase /= tb.denom; - // px4_timestart = mach_absolute_time(); - } + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; - memset(tp, 0, sizeof(*tp)); - - double diff = mach_absolute_time() * px4_timebase; - tp->tv_sec = diff * MAC_NANO; - tp->tv_nsec = diff - (tp->tv_sec * MAC_GIGA); return 0; } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index ba476b5070..41cade1d5d 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -81,9 +81,7 @@ typedef struct { static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; - + pthdata_t *data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 83bb60d796..0286705968 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include #include #include @@ -120,6 +122,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +153,7 @@ #include #include #include +#include #include #include #include diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0f54c9c4c3..b38504c623 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -176,12 +176,9 @@ struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, static void ramtron_attach(void) { - /* find the right spi */ -#ifdef CONFIG_ARCH_BOARD_AEROCORE - struct spi_dev_s *spi = up_spiinitialize(4); -#else - struct spi_dev_s *spi = up_spiinitialize(2); -#endif + /* initialize the right spi */ + struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON); + /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); @@ -269,10 +266,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) } if (!attached) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - at24xxx_attach(); -#else +#ifdef CONFIG_MTD_RAMTRON ramtron_attach(); +#else + at24xxx_attach(); #endif } diff --git a/.cproject b/template_.cproject similarity index 100% rename from .cproject rename to template_.cproject diff --git a/.project b/template_.project similarity index 100% rename from .project rename to template_.project