diff --git a/.gitignore b/.gitignore index 0e553fa365..bd835e271f 100644 --- a/.gitignore +++ b/.gitignore @@ -47,4 +47,4 @@ unittests/build *.generated.h .vagrant *.pretty - +xcode diff --git a/.gitmodules b/.gitmodules index c869803b7a..6a4f815d42 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [submodule "unittests/gtest"] path = unittests/gtest url = https://github.com/sjwilks/gtest.git +[submodule "src/lib/eigen"] + path = src/lib/eigen + url = https://github.com/PX4/eigen.git diff --git a/.travis.yml b/.travis.yml index d659bf6508..0405ba560f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,28 +3,51 @@ language: cpp +# use travis-ci docker based infrastructure +sudo: false + +cache: + directories: + - $HOME/.ccache + +addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - build-essential + - ccache + - cmake + - g++-4.8 + - gcc-4.8 + - genromfs + - libc6-i386 + - libncurses5-dev + - python-argparse + - python-empy + - python-serial + - s3cmd + - texinfo + - zlib1g-dev + before_script: - - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test - - sudo apt-get update -qq - - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi -# Travis specific tools - - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies - - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install python-serial python-argparse python-empy - - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ - - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 - - tar -jxf gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 - - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_7-2014q2/bin:\$PATH" + - wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + - tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 + - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - . ~/.profile - popd - -git: - depth: 500 +# setup ccache + - mkdir -p ~/bin + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/g++-4.8 + - ln -s /usr/bin/ccache ~/bin/gcc-4.8 + - export PATH=~/bin:$PATH env: global: @@ -33,13 +56,9 @@ env: # AWS SECRET: $PX4_AWS_SECRET - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk=" - PX4_AWS_BUCKET=px4-travis - - PX4_EMAIL_SUBJECT="Travis CI result" -# Email address: $PX4_EMAIL - - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - - PX4_REPORT=report.txt - - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: + - ccache -z - arm-none-eabi-gcc --version - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests @@ -47,9 +66,11 @@ script: - echo -en 'travis_fold:end:script.1\\r' - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' - make archives + - ccache -s - echo -en 'travis_fold:end:script.2\\r' - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' - - make -j6 + - make -j4 + - ccache -s - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 @@ -85,4 +106,3 @@ notifications: - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba on_success: always # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always - on_start: false # default: false diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 682c63e471..0a4f532eb0 100644 Binary files a/Documentation/rc_mode_switch.odg and b/Documentation/rc_mode_switch.odg differ diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index e795f48705..cbcc59f723 100644 Binary files a/Documentation/rc_mode_switch.pdf and b/Documentation/rc_mode_switch.pdf differ diff --git a/Images/px4-stm32f4discovery.prototype b/Images/px4-stm32f4discovery.prototype new file mode 100644 index 0000000000..9250691f77 --- /dev/null +++ b/Images/px4-stm32f4discovery.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 99, + "magic": "PX4FWv1", + "description": "Firmware for the STM32F4Discovery board", + "image": "", + "build_time": 0, + "summary": "PX4/STM32F4Discovery", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 551a19928e..9489bfd9a2 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -8,7 +8,14 @@ sh /etc/init.d/rc.vtol_defaults set MIXER firefly6 +set PWM_OUT 123456 + +set MIXER_AUX firefly6 +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1000 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000 -set PWM_OUT 12345678 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 diff --git a/ROMFS/px4fmu_common/init.d/50001_rover b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 similarity index 60% rename from ROMFS/px4fmu_common/init.d/50001_rover rename to ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 index c2f94705cd..a36bac5cc8 100644 --- a/ROMFS/px4fmu_common/init.d/50001_rover +++ b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 @@ -1,10 +1,10 @@ #!nsh # -# Generic rover +# loading default values for the axialracing ax10 # #load some defaults e.g. PWM values -sh /etc/init.d/rc.rover_defaults +sh /etc/init.d/rc.axialracing_ax10_defaults #choose a mixer, for rover control we need a plain passthrough to the servos set MIXER IO_pass diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 434e7870b8..de81795b44 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -287,10 +287,10 @@ fi # -# Ground Rover +# Ground Rover AxialRacing AX10 # if param compare SYS_AUTOSTART 50001 then - sh /etc/init.d/50001_rover + sh /etc/init.d/50001_axialracing_ax10 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps similarity index 51% rename from ROMFS/px4fmu_common/init.d/rc.rover_apps rename to ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps index 1d15b98351..625febd7a4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_apps @@ -6,4 +6,5 @@ ekf_att_pos_estimator start -rover_steering_control start +# disabled the start of steering control app due to use of offboard mode only +# rover_steering_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.rover_defaults rename to ROMFS/px4fmu_common/init.d/rc.axialracing_ax10_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index e6de64054b..156711c26d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -15,4 +15,11 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 + + param set PE_VELNE_NOISE 0.3 + param set PE_VELD_NOISE 0.5 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 0.5 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0002 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 5101acc07f..7a424970f6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -45,7 +45,7 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[i] Mixer: $MIXER_FILE" + echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" else echo "[i] Error loading mixer: $MIXER_FILE" tone_alarm $TUNE_ERR @@ -105,14 +105,14 @@ then set MIXER_AUX_FILE none set OUTPUT_AUX_DEV /dev/pwm_output1 - if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ] then - set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix + set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix else - if [ -f /etc/mixers/$MIXER_AUX.mix ] + if [ -f /etc/mixers/$MIXER_AUX.aux.mix ] then - set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix + set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.aux.mix fi fi @@ -120,7 +120,12 @@ then then if fmu mode_pwm then - mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE + then + echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" + else + echo "[i] Error loading mixer: $MIXER_AUX_FILE" + fi else tone_alarm $TUNE_ERR fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index a14eb5247a..454af8da7a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,10 +7,12 @@ if [ -d /fs/microsd ] then if ver hwcmp PX4FMU_V1 then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 40 -a -b 3 -t + if sdlog2 start -r 40 -a -b 3 -t + then + fi else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -a -b 22 -t + if sdlog2 start -r 200 -a -b 22 -t + then + fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 24d9650a0c..1f34282aec 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -40,6 +40,8 @@ then param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8dff705349..b8a704b90f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -9,12 +9,12 @@ adc start if ver hwcmp PX4FMU_V2 then # External I2C bus - if hmc5883 -C -X start + if hmc5883 -C -T -X start then fi # Internal I2C bus - if hmc5883 -C -I -R 4 start + if hmc5883 -C -T -I -R 4 start then fi @@ -43,7 +43,7 @@ then then fi - if hmc5883 -S -R 8 start + if hmc5883 -C -T -S -R 8 start then fi @@ -115,9 +115,7 @@ then fi # -# Start sensors -> preflight_check +# Start sensors # -if sensors start -then - preflight_check & -fi +sensors start + diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 6dc5a65db7..844d540bf0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -32,6 +32,13 @@ then param set FW_RR_I 0.00 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.02 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 fi set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 59abdc8e33..67ec7821c9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -17,8 +17,8 @@ set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt - set TUNE_ERR ML< 0.1 if self.re_is_a_number.match(defval): defval = self.re_cut_type_specifier.sub('', defval) - param = Parameter() - param.SetField("code", code) - param.SetField("short_desc", code) - param.SetField("type", tp) - param.SetField("default", defval) + param = Parameter(name, tp, defval) + param.SetField("short_desc", name) # If comment was found before the parameter declaration, # inject its data into the newly created parameter. group = "Miscellaneous" @@ -206,8 +214,36 @@ class SourceParser(object): if group not in self.param_groups: self.param_groups[group] = ParameterGroup(group) self.param_groups[group].AddParameter(param) - # Reset parsed comment. - state = None + else: + # Nasty code dup, but this will all go away soon, so quick and dirty (DonLakeFlyer) + m = self.re_px4_parameter_definition.match(line) + if m: + tp, name = m.group(1, 2) + param = Parameter(name, tp) + param.SetField("short_desc", name) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid " + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None def GetParamGroups(self): """ diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index e845cd1b10..07cced4786 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/xmlout.py @@ -1,26 +1,56 @@ -from xml.dom.minidom import getDOMImplementation +import xml.etree.ElementTree as ET import codecs +def indent(elem, level=0): + i = "\n" + level*" " + if len(elem): + if not elem.text or not elem.text.strip(): + elem.text = i + " " + if not elem.tail or not elem.tail.strip(): + elem.tail = i + for elem in elem: + indent(elem, level+1) + if not elem.tail or not elem.tail.strip(): + elem.tail = i + else: + if level and (not elem.tail or not elem.tail.strip()): + elem.tail = i + class XMLOutput(): - def __init__(self, groups): - impl = getDOMImplementation() - xml_document = impl.createDocument(None, "parameters", None) - xml_parameters = xml_document.documentElement + + def __init__(self, groups, board): + xml_parameters = ET.Element("parameters") + xml_version = ET.SubElement(xml_parameters, "version") + xml_version.text = "3" + last_param_name = "" + board_specific_param_set = False for group in groups: - xml_group = xml_document.createElement("group") - xml_group.setAttribute("name", group.GetName()) - xml_parameters.appendChild(xml_group) + xml_group = ET.SubElement(xml_parameters, "group") + xml_group.attrib["name"] = group.GetName() for param in group.GetParams(): - xml_param = xml_document.createElement("parameter") - xml_group.appendChild(xml_param) - for code in param.GetFieldCodes(): - value = param.GetFieldValue(code) - xml_field = xml_document.createElement(code) - xml_param.appendChild(xml_field) - xml_value = xml_document.createTextNode(value) - xml_field.appendChild(xml_value) - self.xml_document = xml_document + if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): + xml_param = ET.SubElement(xml_group, "parameter") + xml_param.attrib["name"] = param.GetName() + xml_param.attrib["default"] = param.GetDefault() + xml_param.attrib["type"] = param.GetType() + last_param_name = param.GetName() + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "board": + if value == board: + board_specific_param_set = True + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + else: + xml_group.remove(xml_param) + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + if last_param_name != param.GetName(): + board_specific_param_set = False + indent(xml_parameters) + self.xml_document = ET.ElementTree(xml_parameters) def Save(self, filename): with codecs.open(filename, 'w', 'utf-8') as f: - self.xml_document.writexml(f, indent=" ", addindent=" ", newl="\n") + self.xml_document.write(f) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index c2da8a2038..152444f84b 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -105,6 +105,7 @@ if args.git_identity != None: if args.parameter_xml != None: f = open(args.parameter_xml, "rb") bytes = f.read() + desc['parameter_xml_size'] = len(bytes) desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 12128a997e..cb2202d529 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -65,6 +65,11 @@ def main(): metavar="FILENAME", help="Create XML file" " (default FILENAME: parameters.xml)") + parser.add_argument("-b", "--board", + nargs='?', + const="", + metavar="BOARD", + help="Board to create xml parameter xml for") parser.add_argument("-w", "--wiki", nargs='?', const="parameters.wiki", @@ -116,7 +121,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups) + out = xmlout.XMLOutput(param_groups, args.board) out.Save(args.xml) # Output to DokuWiki tables diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 43a1167451..3cdd9d1a6f 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -161,6 +161,7 @@ class uploader(object): GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -405,6 +406,12 @@ class uploader(object): raise RuntimeError("Program CRC failed") self.__drawProgressBar(label, 100, 100) + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + # get basic data about the board def identify(self): # make sure we are in sync before starting @@ -472,6 +479,9 @@ class uploader(object): else: self.__verify_v3("Verify ", fw) + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + print("\nRebooting.\n") self.__reboot() self.port.close() @@ -501,6 +511,7 @@ parser = argparse.ArgumentParser(description="Firmware uploader for the PX autop parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() diff --git a/Tools/ros/px4_ros_installation_ubuntu.sh b/Tools/ros/px4_ros_installation_ubuntu.sh deleted file mode 100755 index 7efc400cdd..0000000000 --- a/Tools/ros/px4_ros_installation_ubuntu.sh +++ /dev/null @@ -1,41 +0,0 @@ -#!/bin/sh -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -# main ROS Setup -# following http://wiki.ros.org/indigo/Installation/Ubuntu -# also adding drcsim http://gazebosim.org/tutorials?tut=drcsim_install -# run this file with . ./px4_ros_setup_ubuntu.sh - -## add ROS repository -sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list' - -## add key -wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | \ - sudo apt-key add - - -## Install main ROS pacakges -sudo apt-get update -sudo apt-get -y install ros-indigo-desktop-full -sudo rosdep init -rosdep update - -## Setup environment variables -echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc -. ~/.bashrc - -# get rosinstall -sudo apt-get -y install python-rosinstall - -# additional dependencies -sudo apt-get -y install ros-indigo-octomap-msgs ros-indigo-joy - -## drcsim setup (for models) -### add osrf repository -sudo sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu trusty main" > /etc/apt/sources.list.d/drc-latest.list' - -### add key -wget http://packages.osrfoundation.org/drc.key -O - | sudo apt-key add - - -### install drcsim -sudo apt-get update -sudo apt-get -y install drcsim diff --git a/Tools/ros/px4_workspace_create.sh b/Tools/ros/px4_workspace_create.sh deleted file mode 100755 index cf80bcf8d6..0000000000 --- a/Tools/ros/px4_workspace_create.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/sh -# this script creates a catkin_ws in the current folder -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -mkdir -p catkin_ws/src -cd catkin_ws/src -catkin_init_workspace -cd .. -catkin_make diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh deleted file mode 100755 index 3e4bf06a57..0000000000 --- a/Tools/ros/px4_workspace_setup.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/bash -# License: according to LICENSE.md in the root directory of the PX4 Firmware repository - -# run this script from the root of your catkin_ws -source devel/setup.bash -cd src - -# PX4 Firmware -git clone https://github.com/PX4/Firmware.git - -# euroc simulator -git clone https://github.com/PX4/euroc_simulator.git -cd euroc_simulator -git checkout px4_nodes -cd .. - -# mav comm -git clone https://github.com/PX4/mav_comm.git - -# glog catkin -git clone https://github.com/ethz-asl/glog_catkin.git - -# catkin simple -git clone https://github.com/catkin/catkin_simple.git - -# drcsim (for scenery and models) - -cd .. - -catkin_make diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index 6d115316b2..e5d2887671 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -50,36 +49,40 @@ from manual_input import ManualInput # class ManualInputTest(unittest.TestCase): + def setUp(self): + self.actuator_status = actuator_armed() + self.control_mode = vehicle_control_mode() + # # General callback functions used in tests # def actuator_armed_callback(self, data): - self.actuatorStatus = data + self.actuator_status = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Test arming # def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber('actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - man = ManualInput() + man_in = ManualInput() # Test arming - man.arm() - self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + man_in.arm() + self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl - man.posctl() - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.posctl() + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # Test offboard - man.offboard() - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + man_in.offboard() + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") if __name__ == '__main__': diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index e09550bbc2..9a6c4af783 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -37,23 +37,22 @@ # PKG = 'px4' -import sys import unittest import rospy +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode -from px4.msg import actuator_armed from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint -from sensor_msgs.msg import Joy -from std_msgs.msg import Header +from px4.msg import vehicle_local_position_setpoint from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by directly sending setpoints @@ -66,33 +65,42 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.helper = PX4TestHelper("direct_offboard_posctl_test") + self.helper.setUp() + + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + self.sub_vlp = rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) + self.pub_spt = rospy.Publisher('position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + self.has_pos = False + self.local_position = vehicle_local_position() + self.control_mode = vehicle_control_mode() def tearDown(self): - if (self.fpa): + if self.fpa: self.fpa.stop() + self.helper.tearDown() + # # General callback functions used in tests # + def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -105,12 +113,13 @@ class DirectOffboardPosctlTest(unittest.TestCase): pos.position_valid = True stp = position_setpoint_triplet() stp.current = pos - self.pubSpt.publish(stp) + self.pub_spt.publish(stp) + self.helper.bag_write('px4/position_setpoint_triplet', stp) # does it reach the position in X seconds? count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + while count < timeout: + if self.is_at_position(pos.x, pos.y, pos.z, 0.5): break count = count + 1 self.rate.sleep() @@ -121,22 +130,23 @@ class DirectOffboardPosctlTest(unittest.TestCase): # Test offboard position control # def test_posctl(self): - manIn = ManualInput() + man_in = ManualInput() # arm and go into offboard - manIn.arm() - manIn.offboard() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.arm() + man_in.offboard() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # prepare flight path positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + (0, 0, 0), + (0, 0, -2), + (2, 2, -2), + (2, -2, -2), + (-2, -2, -2), + (2, 2, -2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) @@ -147,12 +157,10 @@ class DirectOffboardPosctlTest(unittest.TestCase): self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, -2, 0.5): break count = count + 1 self.rate.sleep() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index 1a7d843fdc..04ba8a54d3 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -2,8 +2,9 @@ - - + + + @@ -11,8 +12,11 @@ + - - + + + + diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 485de8c416..7de82cc84d 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -35,7 +35,6 @@ # # @author Andreas Antener # -import sys import rospy import threading @@ -48,7 +47,6 @@ from geometry_msgs.msg import Twist from numpy import linalg import numpy as np -import math # # Helper to test if vehicle stays on expected flight path. @@ -62,30 +60,52 @@ class FlightPathAssertion(threading.Thread): # TODO: yaw validation # TODO: fail main test thread # - def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): threading.Thread.__init__(self) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) - self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) - self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) + self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) + self.delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) self.positions = positions - self.tunnelRadius = tunnelRadius - self.yawOffset = yawOffset - self.hasPos = False - self.shouldStop = False + self.tunnel_radius = tunnelRadius + self.yaw_offset = yaw_offset + self.has_pos = False + self.should_stop = False self.center = positions[0] - self.endOfSegment = False + self.end_of_segment = False self.failed = False + self.local_position = vehicle_local_position def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def spawn_indicator(self): - self.deleteModel("indicator") - xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius - self.spawnModel("indicator", xml, "", Pose(), "") + self.delete_model("indicator") + xml = ( + "" + + "" + + "" + + "true" + + "" + + "" + + "0.7" + + "" + + "" + + "%f" + + "" + + "" + + "" + + "1 0 0 0.5" + + "1 0 0 0.5" + + "" + + "" + + "" + + "" + + "") % self.tunnel_radius + + self.spawn_model("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -97,7 +117,7 @@ class FlightPathAssertion(threading.Thread): state.pose = pose state.twist = Twist() state.reference_frame = "" - self.setModelState(state) + self.set_model_state(state) def distance_to_line(self, a, b, pos): v = b - a @@ -111,7 +131,7 @@ class FlightPathAssertion(threading.Thread): c2 = np.dot(v, v) if c2 <= c1: # after b self.center = b - self.endOfSegment = True + self.end_of_segment = True return linalg.norm(pos - b) x = c1 / c2 @@ -120,52 +140,58 @@ class FlightPathAssertion(threading.Thread): return linalg.norm(pos - l) def stop(self): - self.shouldStop = True + self.should_stop = True def run(self): rate = rospy.Rate(10) # 10hz self.spawn_indicator() current = 0 - - while not self.shouldStop: - if (self.hasPos): + count = 0 + while not self.should_stop: + if self.has_pos: # calculate distance to line segment between first two points - # if distances > tunnelRadius + # if distances > tunnel_radius # exit with error - # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # advance current pos if not on the line anymore or distance to next point < tunnel_radius # exit if current pos is now the last position self.position_indicator() - pos = np.array((self.localPosition.x, - self.localPosition.y, - self.localPosition.z)) - aPos = np.array((self.positions[current][0], - self.positions[current][1], - self.positions[current][2])) - bPos = np.array((self.positions[current + 1][0], - self.positions[current + 1][1], - self.positions[current + 1][2])) + pos = np.array((self.local_position.x, + self.local_position.y, + self.local_position.z)) + a_pos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + b_pos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) - dist = self.distance_to_line(aPos, bPos, pos) - bDist = linalg.norm(pos - bPos) + dist = self.distance_to_line(a_pos, b_pos, pos) + b_dist = linalg.norm(pos - b_pos) - rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist)) - if (dist > self.tunnelRadius): - msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + if dist > self.tunnel_radius: + msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z) rospy.logerr(msg) self.failed = True break - if (self.endOfSegment or bDist < self.tunnelRadius): + if self.end_of_segment or b_dist < self.tunnel_radius: rospy.loginfo("next segment") - self.endOfSegment = False + self.end_of_segment = False current = current + 1 - if (current == len(self.positions) - 1): + if current == len(self.positions) - 1: rospy.loginfo("no more positions") break rate.sleep() + count = count + 1 + + if count > 10 and not self.has_pos: # no position after 1 sec + rospy.logerr("no position") + self.failed = True + break diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 9b2471a00e..b7adaa31c4 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -35,7 +35,6 @@ # # @author Andreas Antener # -import sys import rospy from px4.msg import manual_control_setpoint @@ -46,17 +45,12 @@ from std_msgs.msg import Header # # Manual input control helper # -# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else -# the simulator does not instantiate our controller. Probably this whole class will disappear once -# arming works correctly. -# -class ManualInput: +class ManualInput(object): def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + self.pub_mcsp = rospy.Publisher('manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pub_off = rospy.Publisher('offboard_control_mode', offboard_control_mode, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz @@ -81,11 +75,9 @@ class ManualInput: rospy.loginfo("zeroing") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) + self.pub_mcsp.publish(pos) rate.sleep() - count = count + 1 + count = count + 1 pos.r = 1 count = 0 @@ -93,7 +85,7 @@ class ManualInput: rospy.loginfo("arming") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -118,7 +110,7 @@ class ManualInput: rospy.loginfo("triggering posctl") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -147,7 +139,7 @@ class ManualInput: rospy.loginfo("setting offboard mode") time = rospy.get_rostime().now() mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubOff.publish(mode) + self.pub_off.publish(mode) rate.sleep() count = count + 1 @@ -169,7 +161,7 @@ class ManualInput: rospy.loginfo("triggering offboard") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index a52f7ffc1b..7329c1efcb 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -37,79 +37,63 @@ # PKG = 'px4' -import sys import unittest import rospy -import math - -from numpy import linalg -import numpy as np +import rosbag from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput +from px4_test_helper import PX4TestHelper # -# Tests flying a path in offboard control by sending position setpoints +# Tests flying a path in offboard control by sending attitude and thrust setpoints # over MAVROS. # -# For the test to be successful it needs to reach all setpoints in a certain time. -# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# For the test to be successful it needs to cross a certain boundary in time. # class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.helper = PX4TestHelper("mavros_offboard_attctl_test") + self.helper.setUp() + + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback) + self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.control_mode = vehicle_control_mode() + self.local_position = PoseStamped() + + def tearDown(self): + self.helper.tearDown() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data - - - # - # Helper methods - # - def arm(self): - return self.cmdArm(value=True) + self.control_mode = data # # Test offboard position control # def test_attctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_attctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # set some attitude and thrust att = PoseStamped() - att.header = Header() + att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) @@ -121,21 +105,27 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # does it cross expected boundaries in X seconds? count = 0 timeout = 120 - while(count < timeout): + while count < timeout: # update timestamp for each published SP att.header.stamp = rospy.Time.now() - self.pubAtt.publish(att) - self.pubThr.publish(throttle) - - if (self.localPosition.pose.position.x > 5 - and self.localPosition.pose.position.z > 5 - and self.localPosition.pose.position.y < -5): - break - count = count + 1 + + self.pub_att.publish(att) + self.helper.bag_write('mavros/setpoint_attitude/attitude', att) + self.pub_thr.publish(throttle) + self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle) self.rate.sleep() + if (self.local_position.pose.position.x > 5 + and self.local_position.pose.position.z > 5 + and self.local_position.pose.position.y < -5): + break + count = count + 1 + + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count < timeout, "took too long to cross boundaries") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a3739ae5ce..58e7ae2eda 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -37,21 +37,21 @@ # PKG = 'px4' -import sys import unittest import rospy import math +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_local_position_setpoint from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending position setpoints @@ -64,43 +64,53 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.helper = PX4TestHelper("mavros_offboard_posctl_test") + self.helper.setUp() + + rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback) + self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.local_position = PoseStamped() + self.control_mode = vehicle_control_mode() + + def tearDown(self): + self.helper.tearDown() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - if(not self.hasPos): + if not self.has_pos: return False - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + rospy.logdebug("current position %f, %f, %f" % + (self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) + desired = np.array((x, y, z)) - pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + pos = np.array((self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): # set a position setpoint pos = PoseStamped() - pos.header = Header() + pos.header = Header() pos.header.frame_id = "base_footprint" pos.pose.position.x = x pos.pose.position.y = y @@ -114,59 +124,47 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 - while(count < timeout): + while count < timeout: # update timestamp for each published SP pos.header.stamp = rospy.Time.now() - self.pubSpt.publish(pos) - - if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + self.pub_spt.publish(pos) + self.helper.bag_write('mavros/setpoint_position/local', pos) + + if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to get to position") - def arm(self): - return self.cmdArm(value=True) - # # Test offboard position control # def test_posctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_posctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # prepare flight path positions = ( - (0,0,0), - (2,2,2), - (2,-2,2), - (-2,-2,2), - (2,2,2)) + (0, 0, 0), + (2, 2, 2), + (2, -2, 2), + (-2, -2, 2), + (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - - # does it hold the position for Y seconds? - positionHeld = True + count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, 2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, 2, 0.5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count == timeout, "position could not be held") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index 4651f0dc9c..cdafc967c5 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -2,8 +2,9 @@ - - + + + @@ -11,9 +12,14 @@ + + + + - - - + + + + diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/demo_tests/px4_test_helper.py new file mode 100644 index 0000000000..4dc8866341 --- /dev/null +++ b/integrationtests/demo_tests/px4_test_helper.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# 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 Andreas Antener +# +PKG = 'px4' + +import unittest +import rospy +import rosbag + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude +from px4.msg import vehicle_local_position_setpoint + +from threading import Condition + +# +# Test helper +# +class PX4TestHelper(object): + + def __init__(self, test_name): + self.test_name = test_name + + def setUp(self): + self.condition = Condition() + self.closed = False + + rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4") + + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", + vehicle_local_position, self.vehicle_position_callback) + self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint", + vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback) + self.sub_va = rospy.Subscriber("iris/vehicle_attitude", + vehicle_attitude, self.vehicle_attitude_callback) + self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint", + vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback) + + + def tearDown(self): + try: + self.condition.acquire() + self.closed = True + + self.sub_vlp.unregister() + self.sub_vasp.unregister() + self.sub_va.unregister() + self.sub_vlps.unregister() + self.bag.close() + + finally: + self.condition.release() + + def vehicle_position_callback(self, data): + self.bag_write('px4/vehicle_local_position', data) + + def vehicle_attitude_setpoint_callback(self, data): + self.bag_write('px4/vehicle_attitude_setpoint', data) + + def vehicle_attitude_callback(self, data): + self.bag_write('px4/vehicle_attitude', data) + + def vehicle_local_position_setpoint_callback(self, data): + self.bag_write('px4/vehicle_local_position_setpoint', data) + + def bag_write(self, topic, data): + try: + self.condition.acquire() + if not self.closed: + self.bag.write(topic, data) + else: + rospy.logwarn("Trying to write to bag but it's already closed") + finally: + self.condition.release() + diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 3173e7cf1a..56030dd1f4 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,14 +1,19 @@ + - + + + - + - + + + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 22bfb0c799..7243e7476e 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,16 +3,19 @@ - - + + + - + - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 717244abf5..1309529b63 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -1,9 +1,14 @@ - + - - - - + + + + + + + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 9ff7f7d1fa..0371306ce5 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -1,9 +1,14 @@ - + - - - - + + + + + + + + + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index c636868eb2..89f27c901f 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 8a4c128b90..0e80208e67 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch index e4f9f6532c..d0e824d4b5 100644 --- a/launch/gazebo_iris_house_world.launch +++ b/launch/gazebo_iris_house_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index 22d55502d3..e75e57b877 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/iris.launch b/launch/iris.launch index be33cb85f4..bf5b099b64 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,11 @@ + - + + + - + @@ -10,6 +13,8 @@ + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 40010a2735..f452f09453 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -1,16 +1,16 @@ - - + + + + - - - + @@ -18,4 +18,5 @@ + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index bc0e377715..6882f24137 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,7 @@ + - + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index f124082aa9..66c30d186f 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index 6355b87be9..c686eba390 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/makefiles/board_px4-stm32f4discovery.mk b/makefiles/board_px4-stm32f4discovery.mk new file mode 100644 index 0000000000..fe761ba444 --- /dev/null +++ b/makefiles/board_px4-stm32f4discovery.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the PX4_STM32F4DISCOVERY +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = PX4_STM32F4DISCOVERY + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 0d3934bd01..c906d54189 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -27,11 +27,10 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/boardinfo +MODULES += systemcmds/ver MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4-stm32f4discovery_default.mk b/makefiles/config_px4-stm32f4discovery_default.mk new file mode 100644 index 0000000000..8f73a7f047 --- /dev/null +++ b/makefiles/config_px4-stm32f4discovery_default.mk @@ -0,0 +1,92 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the PX4_STM32F4DISCOVERY firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4-stm32f4discovery + +# +# System commands +# +MODULES += systemcmds/bl_update +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/ver + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion +MODULES += platforms/nuttx + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e3515a927e..55b73ffde3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -45,7 +45,6 @@ MODULES += systemcmds/mtd MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f414186d15..cbfda9735f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -50,11 +50,9 @@ MODULES += drivers/gimbal # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 29eb680960..b98532f715 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -48,11 +48,9 @@ MODULES += drivers/px4flow # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 22563838bd..154f7e5bad 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,11 +42,9 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 21e8739aa1..af3ca249e5 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -180,11 +180,6 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) EXTRA_CLEANS = -# -# Extra defines for compilation -# -export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC) - # # Append the per-board driver directory to the header search path. # @@ -499,7 +494,7 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 2c53028124..3b9fefb3ef 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -296,6 +296,9 @@ endef # # - compile an empty file to generate a suitable object file # - relink the object and insert the binary file +# - extract the length +# - create const unsigned $3_len with the extracted length as its value and compile it to an object file +# - link the two generated object files together # - edit symbol names to suit # # NOTE: exercise caution using this with absolute pathnames; it looks @@ -320,11 +323,14 @@ define BIN_TO_OBJ @$(MKDIR) -p $(dir $2) $(Q) $(ECHO) > $2.c $(call COMPILE,$2.c,$2.c.o) - $(Q) $(LD) -r -o $2 $2.c.o -b binary $1 + $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 + $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c + $(call COMPILE,$2.c,$2.c.o) + $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o $(Q) $(OBJCOPY) $2 \ --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \ - --redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \ + --strip-symbol $(call BIN_SYM_PREFIX,$1)_size \ --strip-symbol $(call BIN_SYM_PREFIX,$1)_end \ --rename-section .data=.rodata - $(Q) $(REMOVE) $2.c $2.c.o + $(Q) $(REMOVE) $2.c $2.c.o $2.bin.o endef diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 29b415688e..dd7710bf72 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -33,7 +33,8 @@ upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) upload-serial-aerocore: openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' - +upload-serial-px4-stm32f4discovery: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 7ae438b86e..475e7cc434 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 7ae438b86ea983e95af5f092e45a5d0f9d093c74 +Subproject commit 475e7cc4348b207ac3efed45eb61160d23ac7a26 diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg new file mode 100644 index 0000000000..114e843b0e --- /dev/null +++ b/msg/mc_att_ctrl_status.msg @@ -0,0 +1,5 @@ +uint64 timestamp # in microseconds since system start + +float32 roll_rate_integ # roll rate inegrator +float32 pitch_rate_integ # pitch rate integrator +float32 yaw_rate_integ # yaw rate integrator diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 18df3252fe..2e001cac73 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -112,6 +112,7 @@ bool condition_airspeed_valid # set to true by the commander app if there is a bool condition_landed # true if vehicle is landed, always true if disarmed bool condition_power_input_valid # set if input power is valid float32 avionics_power_rail_voltage # voltage of the avionics power rail +bool usb_connected # status of the USB power supply bool rc_signal_found_once bool rc_signal_lost # true if RC reception lost diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h index 8705c1bc25..6dc85735a5 100644 --- a/nuttx-configs/aerocore/include/board.h +++ b/nuttx-configs/aerocore/include/board.h @@ -145,7 +145,7 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ @@ -203,7 +203,7 @@ /* * SPI */ -/* PA[4-7] SPI1 broken out on J12 */ +/* PA[4-7] SPI1 broken out on J12 */ #define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/aerocore/src/empty.c +++ b/nuttx-configs/aerocore/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h new file mode 100644 index 0000000000..0412c3aa6d --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -0,0 +1,273 @@ +/************************************************************************************ + * configs/stm32f4discovery/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The STM32F4 Discovery board features a single 8MHz crystal. Space is provided + * for a 32kHz RTC backup crystal, but it is not stuffed. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - 32.768 kHz + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * px4fmu-v1. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* UART2: + * + * The STM32F4 Discovery has no on-board serial devices, but the console is + * brought out to PA2 (TX) and PA3 (RX) for connection to an external serial device. + * (See the README.txt file for other options) + */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* SPI - There is a MEMS device on SPI1 using these pins: */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +/************************************************************************************ + * Name: stm32_ledinit, stm32_setled, and stm32_setleds + * + * Description: + * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If + * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to + * control the LEDs from user applications. + * + ************************************************************************************/ + +#ifndef CONFIG_ARCH_LEDS +EXTERN void stm32_ledinit(void); +EXTERN void stm32_setled(int led, bool ledon); +EXTERN void stm32_setleds(uint8_t ledset); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs new file mode 100644 index 0000000000..e70320aaa4 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/src/systemcmds/preflight_check/module.mk b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig similarity index 75% rename from src/systemcmds/preflight_check/module.mk rename to nuttx-configs/px4-stm32f4discovery/nsh/appconfig index 0cb2a4cd09..f14ab4044b 100644 --- a/src/systemcmds/preflight_check/module.mk +++ b/nuttx-configs/px4-stm32f4discovery/nsh/appconfig @@ -1,6 +1,8 @@ ############################################################################ +# configs/px4fmu/nsh/appconfig # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -12,7 +14,7 @@ # 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 +# 3. Neither the name NuttX nor the names of its contributors may be # used to endorse or promote products derived from this software # without specific prior written permission. # @@ -31,14 +33,16 @@ # ############################################################################ -# -# Pre-flight check. Locks down system for a few systems with blinking leds -# and buzzer if the sensors do not report an OK status. -# +# Path to example in apps/examples containing the user_start entry point -MODULE_COMMAND = preflight_check -SRCS = preflight_check.c +CONFIGURED_APPS += examples/nsh -MAXOPTIMIZATION = -Os +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline -MODULE_STACKSIZE = 1800 +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig new file mode 100644 index 0000000000..6a2470bea9 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -0,0 +1,897 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +CONFIG_ARCH_CHIP_STM32F407VG=y +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_STM32F427 is not set +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +# CONFIG_STM32_SPI2 is not set +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_USART1 is not set +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +# CONFIG_STM32_UART7 is not set +# CONFIG_STM32_UART8 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_WWDG is not set +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=500 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=44 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART2=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +# CONFIG_CDCACM_CONSOLE is not set +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +# CONFIG_FAT_LCNAMES is not set +# CONFIG_FAT_LFN is not set +# CONFIG_FS_FATTIME is not set +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=0 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh new file mode 100755 index 0000000000..265520997d --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4-stm32f4discovery/scripts/ld.script b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script new file mode 100644 index 0000000000..f39a95c450 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/scripts/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu-v1/scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F40VGG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4-stm32f4discovery/src/Makefile b/nuttx-configs/px4-stm32f4discovery/src/Makefile new file mode 100644 index 0000000000..d4276f7fc5 --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4-stm32f4discovery/src/empty.c b/nuttx-configs/px4-stm32f4discovery/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx-configs/px4-stm32f4discovery/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index ff1c634244..9e6d50a8b9 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -209,7 +209,7 @@ /* UART DMA configuration for USART1/6 */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - + /* * CAN * diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cd410051c7..389cb5ab5b 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4fmu-v1/src/empty.c +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3b3c6fa707..52668cacd2 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -146,21 +146,21 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ #define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) #define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) -/* SDIO dividers. Note that slower clocking is required when DMA is disabled +/* SDIO dividers. Note that slower clocking is required when DMA is disabled * in order to avoid RX overrun/TX underrun errors due to delayed responses * to service FIFOs in interrupt driven mode. These values have not been * tuned!!! * * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz */ - + #define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz @@ -168,9 +168,9 @@ */ #ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) #else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz @@ -227,10 +227,10 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* +/* * CAN * - * CAN1 is routed to the onboard transceiver. + * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ #define GPIO_CAN1_RX GPIO_CAN1_RX_3 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 4ccc5dacb6..d331aa26a7 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=44 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1ce..b04ad89a6c 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -66,12 +66,20 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ EXTERN(abort) +EXTERN(_bootdelay_signature) SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; *(.text .text.*) *(.fixup) *(.gnu.warning) diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4fmu-v2/src/empty.c +++ b/nuttx-configs/px4fmu-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h index 8150792665..ee8a9d1ba7 100755 --- a/nuttx-configs/px4io-v1/include/board.h +++ b/nuttx-configs/px4io-v1/include/board.h @@ -143,7 +143,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4io-v1/src/empty.c +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 17e77918b4..b70056f828 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -138,7 +138,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c index ace900866c..5de10699fb 100644 --- a/nuttx-configs/px4io-v2/src/empty.c +++ b/nuttx-configs/px4io-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index d6a88714b6..cdaf244dd1 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval, const char* path); + Airspeed(int bus, int address, unsigned conversion_interval, const char *path); virtual ~Airspeed(); virtual int init(); @@ -108,8 +108,8 @@ private: perf_counter_t _buffer_overflows; /* this class has pointer data members and should not be copied */ - Airspeed(const Airspeed&); - Airspeed& operator=(const Airspeed&); + Airspeed(const Airspeed &); + Airspeed &operator=(const Airspeed &); protected: virtual int probe(); diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7d4b7d880b..53d98ba9a1 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -108,8 +108,9 @@ usage(const char *reason) */ int ardrone_interface_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h index 78b603b63a..dcc49a4727 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.h +++ b/src/drivers/ardrone_interface/ardrone_motor_control.h @@ -85,7 +85,8 @@ int ar_init_motors(int ardrone_uart, int gpio); /** * Set LED pattern. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); /** * Mix motors and output actuators diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index a958fc60d3..6d1b3f6ca5 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,6 +84,8 @@ #define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage +#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -114,6 +116,11 @@ public: */ virtual int init(); + /** + * ioctl for retrieving battery capacity and time to empty + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** * Test device * @@ -179,6 +186,7 @@ private: orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery + uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -197,7 +205,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _reports(nullptr), _batt_topic(-1), _batt_orb_id(nullptr), - _start_time(0) + _start_time(0), + _batt_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -247,6 +256,31 @@ BATT_SMBUS::init() return ret; } +int +BATT_SMBUS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -ENODEV; + + switch (cmd) { + case BATT_SMBUS_GET_CAPACITY: + + /* return battery capacity as uint16 */ + if (_enabled) { + *((uint16_t *)arg) = _batt_capacity; + ret = OK; + } + + break; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + int BATT_SMBUS::test() { @@ -263,7 +297,8 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a, + (float)status.discharged_mah, (int)_batt_capacity); } } @@ -279,6 +314,7 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; + int16_t orig_addr = get_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { @@ -293,6 +329,9 @@ BATT_SMBUS::search() usleep(1); } + // restore original i2c address + set_address(orig_addr); + // display completion message if (found_slave) { warnx("Done."); @@ -364,11 +403,27 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - usleep(1); uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | + (uint32_t)buff[0])) / 1000.0f; + } + + // read battery design capacity + if (_batt_capacity == 0) { + if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) { + _batt_capacity = tmp; + } + } + + // read remaining capacity + if (_batt_capacity > 0) { + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (tmp < _batt_capacity) { + new_report.discharged_mah = _batt_capacity - tmp; + } + } } // publish to orb @@ -430,8 +485,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ { uint8_t buff[max_len + 2]; // buffer to hold results - usleep(1); - // read bytes including PEC int ret = transfer(®, 1, buff, max_len + 2); diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index e329bd9d1a..0fa474a8c1 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -136,8 +136,8 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); break; diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 776a2071ed..19518d73d9 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h new file mode 100644 index 0000000000..dd66abc19f --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * PX4-STM32F4Discovery internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + +/* SPI chip selects */ +#define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4-stm32f4discovery/module.mk b/src/drivers/boards/px4-stm32f4discovery/module.mk new file mode 100644 index 0000000000..6b75c08e92 --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/module.mk @@ -0,0 +1,7 @@ +# +# Board-specific startup code for the PX4-STM32F4Discovery +# + +SRCS = px4discovery_init.c \ + px4discovery_usb.c \ + px4discovery_led.c diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c new file mode 100644 index 0000000000..fb1b282364 --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_init.c + * + * PX4-stm32f4discovery specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + return OK; +} diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c new file mode 100644 index 0000000000..1f42843edf --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_led.c + * + * PX4-stm32f4discovery LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c new file mode 100644 index 0000000000..887ca67eff --- /dev/null +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4discovery_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4-STM32F4Discovery board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 882ec6aa2d..17fa9c542e 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -52,7 +52,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 17e6862f7c..16f94a9f23 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -90,8 +90,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 273af1023a..6188e29ae1 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 8292da9e1c..5a02a97164 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -87,7 +87,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_RELAY1_EN); stm32_configgpio(GPIO_RELAY2_EN); - /* turn off - all leds are active low */ + /* turn off - all leds are active low */ stm32_gpiowrite(GPIO_LED1, true); stm32_gpiowrite(GPIO_LED2, true); stm32_gpiowrite(GPIO_LED3, true); diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 10a93be0bf..99baee41df 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -50,7 +50,7 @@ /* these headers are not C++ safe */ #include #include - + /****************************************************************************** * Definitions ******************************************************************************/ @@ -117,7 +117,7 @@ #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) -/* +/* * High-resolution timer */ #define HRT_TIMER 1 /* use timer1 for the HRT */ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 5c3343ccca..fd7eb33c3e 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -126,7 +126,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ stm32_configgpio(GPIO_SBUS_OUTPUT); - + /* sbus output enable is active low - disable it by default */ stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 7bb4ae1aff..cb13fed83c 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -63,7 +63,7 @@ public: static int set_bus_clock(unsigned bus, unsigned clock_hz); static unsigned int _bus_clocks[3]; - + protected: /** * The number of times a read or write operation will be retried on @@ -144,8 +144,8 @@ private: uint32_t _frequency; struct i2c_dev_s *_dev; - I2C(const device::I2C&); - I2C operator=(const device::I2C&); + I2C(const device::I2C &); + I2C operator=(const device::I2C &); }; } // namespace device diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 2ff91d5d00..fddef36263 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -35,7 +35,7 @@ * @file drv_airspeed.h * * Airspeed driver interface. - * + * * @author Simon Wilks */ diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index f12e2bfb3a..57af0a0b68 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,3 +45,13 @@ /* device path */ #define BATT_SMBUS0_DEVICE_PATH "/dev/batt_smbus0" + +/* + * ioctl() definitions + */ + +#define _BATT_SMBUS_IOCBASE (0x2e00) +#define _BATT_SMBUS_IOC(_n) (_IOC(_BATT_SMBUS_IOCBASE, _n)) + +/** retrieve battery capacity */ +#define BATT_SMBUS_GET_CAPACITY _BATT_SMBUS_IOC(1) diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 7258c9e841..8568436d35 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -60,7 +60,7 @@ /** play the numbered script in (arg), repeating forever */ #define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving , , arg[0-2] * diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index be9604b6ee..f18c8162d7 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -110,9 +110,13 @@ /* no GPIO driver on the PX4IOv2 board */ #endif +#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY +/* no GPIO driver on the PX4_STM32F4DISCOVERY board */ +#endif + #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ - !defined(CONFIG_ARCH_BOARD_AEROCORE) + !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8bfc90c64b..a40943d3f2 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -127,7 +127,8 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may * jitter but should not drift. */ -__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); +__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, + hrt_callout callout, void *arg); /* * If this returns true, the entry has been invoked and removed from the callout list, diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 7fcff9d382..3c12216a62 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -63,6 +63,7 @@ struct mag_report { float z; float range_ga; float scaling; + float temperature; int16_t x_raw; int16_t y_raw; @@ -128,4 +129,7 @@ ORB_DECLARE(sensor_mag); /** determine if mag is external or onboard */ #define MAGIOCGEXTERNAL _MAGIOC(11) +/** enable/disable temperature compensation */ +#define MAGIOCSTEMPCOMP _MAGIOC(12) + #endif /* _DRV_MAG_H */ diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index e0b16fa5cd..c1db6b534b 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -36,7 +36,7 @@ /** * @file drv_orb_dev.h - * + * * uORB published object driver. */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 0dcb10a7b1..00e3683184 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -117,7 +117,7 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_ENUM_COUNT }; -/* +/* structure passed to OREOLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h index 778d9fcf5b..47b84e6e1e 100644 --- a/src/drivers/drv_pwm_input.h +++ b/src/drivers/drv_pwm_input.h @@ -46,6 +46,6 @@ __BEGIN_DECLS /* * Initialise the timer */ -__EXPORT extern int pwm_input_main(int argc, char * argv[]); +__EXPORT extern int pwm_input_main(int argc, char *argv[]); __END_DECLS diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 35e9619f07..6271ad2086 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -78,7 +78,7 @@ __BEGIN_DECLS /** * Highest PWM allowed as the minimum PWM */ -#define PWM_HIGHEST_MIN 1300 +#define PWM_HIGHEST_MIN 1600 /** * Highest maximum PWM in us @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1700 +#define PWM_LOWEST_MAX 1400 /** * Do not output a channel with this value diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index d44728a712..a24d8814fe 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -65,7 +65,7 @@ /** * Maximum RSSI value */ -#define RC_INPUT_RSSI_MAX 255 +#define RC_INPUT_RSSI_MAX 100 /** * @addtogroup topics diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index b10caf56a1..0633768684 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -58,7 +58,7 @@ /** play the numbered script in (arg), repeating forever */ #define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving , , arg[0-2] * @@ -79,7 +79,7 @@ #define RGBLED_SET_PATTERN _RGBLEDIOC(7) -/* +/* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 890fada168..de22a888d3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -222,6 +222,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); @@ -232,7 +233,7 @@ void frsky_send_frame2(int uart) lon = frsky_format_gps(fabsf(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) - * 25.0f / 46.0f; + * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; } diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ccda4c0a56..1e27309d83 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -121,8 +121,12 @@ private: int _vehicle_command_sub; int _att_sub; - bool _attitude_compensation; + bool _attitude_compensation_roll; + bool _attitude_compensation_pitch; + bool _attitude_compensation_yaw; bool _initialized; + bool _control_cmd_set; + bool _config_cmd_set; orb_advert_t _actuator_controls_2_topic; @@ -130,6 +134,9 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + struct vehicle_command_s _control_cmd; + struct vehicle_command_s _config_cmd; + /** * Initialise the automatic measurement state machine and start it. * @@ -169,7 +176,9 @@ Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), - _attitude_compensation(true), + _attitude_compensation_roll(true), + _attitude_compensation_pitch(true), + _attitude_compensation_yaw(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), @@ -221,7 +230,9 @@ Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case GIMBALIOCATTCOMPENSATE: - _attitude_compensation = (arg != 0); + _attitude_compensation_roll = (arg != 0); + _attitude_compensation_pitch = (arg != 0); + _attitude_compensation_yaw = (arg != 0); return OK; default: @@ -286,22 +297,30 @@ Gimbal::cycle() float yaw = 0.0f; - if (_attitude_compensation) { - if (_att_sub < 0) { - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - } + if (_att_sub < 0) { + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } - vehicle_attitude_s att; + vehicle_attitude_s att; - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + if (_attitude_compensation_roll) { roll = -att.roll; - pitch = -att.pitch; - yaw = att.yaw; - updated = true; } + if (_attitude_compensation_pitch) { + pitch = -att.pitch; + updated = true; + } + + if (_attitude_compensation_yaw) { + yaw = att.yaw; + updated = true; + } + + struct vehicle_command_s cmd; bool cmd_updated; @@ -312,22 +331,47 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); - VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)cmd.param7; - debug("cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", cmd.command, mountMode, (double)cmd.param1, (double)cmd.param2); + if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + _control_cmd = cmd; + _control_cmd_set = true; - /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ - roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param1; - pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param2; - yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * cmd.param3; - - updated = true; + } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _config_cmd = cmd; + _config_cmd_set = true; } - if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { - float gimbalDirectionQuat[] = {cmd.param1, cmd.param2, cmd.param3, cmd.param4}; + } + + if (_config_cmd_set) { + + _config_cmd_set = false; + + _attitude_compensation_roll = (int)_config_cmd.param2 == 1; + _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; + _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; + + } + + if (_control_cmd_set) { + + VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ + roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; + pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; + yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; + + updated = true; + } + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); roll += gimablDirectionEuler(0); @@ -336,6 +380,7 @@ Gimbal::cycle() updated = true; } + } if (updated) { diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f42c968d37..064ce20e5f 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -716,7 +716,18 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_PVT: UBX_TRACE_RXMSG("Rx NAV-PVT\n"); - _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + //Check if position fix flag is good + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) + { + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->vel_ned_valid = true; + } + else + { + _gps_position->fix_type = 0; + _gps_position->vel_ned_valid = false; + } + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; _gps_position->lat = _buf.payload_rx_nav_pvt.lat; @@ -732,11 +743,14 @@ UBX::payload_rx_done(void) _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; - _gps_position->vel_ned_valid = true; _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; + //Check if time and date fix flags are good + if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; @@ -813,6 +827,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); + if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9d28b48d46..961ec47246 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -402,7 +402,7 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20be..3a3848446d 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -94,6 +94,10 @@ #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 +/* temperature on hmc5983 only */ +#define ADDR_TEMP_OUT_MSB 0x31 +#define ADDR_TEMP_OUT_LSB 0x32 + /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ #define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ @@ -110,6 +114,8 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ +#define HMC5983_TEMP_SENSOR_ENABLE (1 << 7) + enum HMC5883_BUS { HMC5883_BUS_ALL = 0, HMC5883_BUS_I2C_INTERNAL, @@ -176,6 +182,8 @@ private: uint8_t _range_bits; uint8_t _conf_reg; + uint8_t _temperature_counter; + uint8_t _temperature_error_count; /** * Initialise the automatic measurement state machine and start it. @@ -218,6 +226,11 @@ private: */ int set_excitement(unsigned enable); + /** + * enable hmc5983 temperature compensation + */ + int set_temperature_compensation(unsigned enable); + /** * Set the sensor range. * @@ -358,7 +371,9 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _rotation(rotation), _last_report{0}, _range_bits(0), - _conf_reg(0) + _conf_reg(0), + _temperature_counter(0), + _temperature_error_count(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; @@ -722,6 +737,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case MAGIOCSTEMPCOMP: + return set_temperature_compensation(arg); + case DEVIOCGDEVICEID: return _interface->ioctl(cmd, dummy); @@ -848,6 +866,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -884,6 +906,45 @@ HMC5883::collect() goto out; } + /* get measurements from the device */ + new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { + /* + if temperature compensation is enabled read the + temperature too. + + We read the temperature every 10 samples to avoid + excessive I2C traffic + */ + if (_temperature_counter++ == 10) { + uint8_t raw_temperature[2]; + + _temperature_counter = 0; + + ret = _interface->read(ADDR_TEMP_OUT_MSB, + raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16*8.0f)); + _temperature_error_count = 0; + } else { + _temperature_error_count++; + if (_temperature_error_count == 10) { + /* + it probably really is an old HMC5883, + and can't do temperature. Disable it + */ + _temperature_error_count = 0; + debug("disabling temperature compensation"); + set_temperature_compensation(0); + } + } + } else { + new_report.temperature = _last_report.temperature; + } + } + /* * RAW outputs * @@ -907,17 +968,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { @@ -1223,6 +1288,60 @@ int HMC5883::set_excitement(unsigned enable) return !(_conf_reg == conf_reg_ret); } + +/* + enable/disable temperature compensation on the HMC5983 + + Unfortunately we don't yet know of a way to auto-detect the + difference between the HMC5883 and HMC5983. Both of them do + temperature sensing, but only the 5983 does temperature + compensation. We have noy yet found a behaviour that can be reliably + distinguished by reading registers to know which type a particular + sensor is + + update: Current best guess is that many sensors marked HMC5883L on + the package are actually 5983 but without temperature compensation + tables. Reading the temperature works, but the mag field is not + automatically adjusted for temperature. We suspect that there may be + some early 5883L parts that don't have the temperature sensor at + all, although we haven't found one yet. The code that reads the + temperature looks for 10 failed transfers in a row and disables the + temperature sensor if that happens. It is hoped that this copes with + the genuine 5883L parts. + */ +int HMC5883::set_temperature_compensation(unsigned enable) +{ + int ret; + /* get current config */ + ret = read_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + if (enable != 0) { + _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { + _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; + } + + ret = write_reg(ADDR_CONF_A, _conf_reg); + + if (OK != ret) { + perf_count(_comms_errors); + return -EIO; + } + + uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { + perf_count(_comms_errors); + return -EIO; + } + + return conf_reg_ret == _conf_reg; +} + int HMC5883::write_reg(uint8_t reg, uint8_t val) { @@ -1265,6 +1384,7 @@ HMC5883::print_info() printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); + printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1307,6 +1427,7 @@ void test(enum HMC5883_BUS busid); void reset(enum HMC5883_BUS busid); int info(enum HMC5883_BUS busid); int calibrate(enum HMC5883_BUS busid); +int temp_enable(HMC5883_BUS busid, bool enable); void usage(); /** @@ -1371,8 +1492,6 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) if (!started) errx(1, "driver start failed"); - - exit(0); } /** @@ -1517,12 +1636,7 @@ int calibrate(enum HMC5883_BUS busid) close(fd); - if (ret == OK) { - errx(0, "PASS"); - - } else { - errx(1, "FAIL"); - } + return ret; } /** @@ -1548,6 +1662,28 @@ reset(enum HMC5883_BUS busid) exit(0); } + +/** + * enable/disable temperature compensation + */ +int +temp_enable(enum HMC5883_BUS busid, bool enable) +{ + struct hmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + err(1, "set temperature compensation failed"); + + close(fd); + return 0; +} + /** * Print a little info about the driver. */ @@ -1583,8 +1719,9 @@ hmc5883_main(int argc, char *argv[]) enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; + bool temp_compensation = false; - while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); @@ -1603,6 +1740,9 @@ hmc5883_main(int argc, char *argv[]) case 'C': calibrate = true; break; + case 'T': + temp_compensation = true; + break; default: hmc5883::usage(); exit(0); @@ -1616,16 +1756,15 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); - if (calibrate) { - if (hmc5883::calibrate(busid) == 0) { - errx(0, "calibration successful"); - - } else { - errx(1, "calibration failed"); - } - } else { - exit(0); + if (calibrate && hmc5883::calibrate(busid) != 0) { + errx(1, "calibration failed"); } + if (temp_compensation) { + // we consider failing to setup temperature + // compensation as non-fatal + hmc5883::temp_enable(busid, true); + } + exit(0); } /* @@ -1640,6 +1779,14 @@ hmc5883_main(int argc, char *argv[]) if (!strcmp(verb, "reset")) hmc5883::reset(busid); + /* + * enable/disable temperature compensation + */ + if (!strcmp(verb, "tempoff")) + hmc5883::temp_enable(busid, false); + if (!strcmp(verb, "tempon")) + hmc5883::temp_enable(busid, true); + /* * Print driver information. */ @@ -1658,5 +1805,5 @@ hmc5883_main(int argc, char *argv[]) } } - errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'"); } diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index e91e91fc09..9607fe6149 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,4 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; -typedef device::Device* (*HMC5883_constructor)(int); +typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 1aade450b1..4ac7e4bdfb 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -197,7 +197,7 @@ hott_sensors_thread_main(int argc, char *argv[]) int hott_sensors_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "missing command\n%s", commandline_usage); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 17a24d1041..43df0cb8cc 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -223,7 +223,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) int hott_telemetry_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "missing command\n%s", commandline_usage); } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8b..7687236408 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1029,9 +1029,16 @@ L3GD20::measure() report.temperature_raw = raw_report.temp; - report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xraw_f = report.x_raw; + float yraw_f = report.y_raw; + float zraw_f = report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.x = _gyro_filter_x.apply(report.x); report.y = _gyro_filter_y.apply(report.y); @@ -1039,9 +1046,6 @@ L3GD20::measure() report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; - // apply user specified rotation - rotate_3f(_rotation, report.x, report.y, report.z); - report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b5f01b942f..e594c92a11 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -305,6 +305,9 @@ private: float _last_accel[3]; uint8_t _constant_accel_count; + // last temperature value + float _last_temperature; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -567,6 +570,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), + _last_temperature(0), _checked_next(0) { @@ -711,7 +715,7 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -1507,6 +1511,9 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + // use the temperature from the last mag reading + accel_report.temperature = _last_temperature; + // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on @@ -1517,9 +1524,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed @@ -1555,9 +1569,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1584,6 +1595,7 @@ LSM303D::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; + int16_t temperature; uint8_t status; int16_t x; int16_t y; @@ -1599,7 +1611,7 @@ LSM303D::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); /* @@ -1623,19 +1635,29 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + /* apply user specified rotation */ + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); + /* remember the temperature. The datasheet isn't clear, but it + * seems to be a signed offset from 25 degrees C in units of 0.125C + */ + _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + mag_report.temperature = _last_temperature; _mag_reports->force(&mag_report); - /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1672,6 +1694,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.2f\n", (double)_last_temperature); } void diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp index 270635f407..e8dd4ee9c5 100644 --- a/src/drivers/md25/BlockSysIdent.hpp +++ b/src/drivers/md25/BlockSysIdent.hpp @@ -1,7 +1,8 @@ #include #include -class BlockSysIdent : public control::Block { +class BlockSysIdent : public control::Block +{ public: BlockSysIdent(); private: diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 7e5904d050..d25d16fa15 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -97,8 +97,9 @@ usage(const char *reason) int md25_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index cb463eb59f..3b8c4ee777 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -536,7 +536,7 @@ MK::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea85776..b6642e2bbd 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -284,6 +284,9 @@ private: // self test volatile bool _in_factory_test; + // last temperature reading for print_info() + float _last_temperature; + /** * Start automatic measurement. */ @@ -359,7 +362,7 @@ private: * @param max_g The maximum G value the range must support. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int set_accel_range(unsigned max_g); /** * Swap a 16-bit value read from the MPU6000 to native byte order. @@ -518,7 +521,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false) + _in_factory_test(false), + _last_temperature(0) { // disable debug() calls _debug_enabled = false; @@ -713,37 +717,7 @@ int MPU6000::reset() _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; - // product-specific scaling - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - break; - - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug - default: - // Accel scale 8g (4096 LSB/g) - write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); - break; - } - - // Correct accel scale factors of 4096 LSB/g - // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (MPU6000_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + set_accel_range(8); usleep(1000); @@ -1297,11 +1271,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_m_s2 = 8.0f * 9.81f; - return -EINVAL; + return set_accel_range(arg); + case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); @@ -1451,44 +1422,46 @@ MPU6000::write_checked_reg(unsigned reg, uint8_t value) } int -MPU6000::set_range(unsigned max_g) +MPU6000::set_accel_range(unsigned max_g_in) { -#if 0 - uint8_t rangebits; - float rangescale; - - if (max_g > 16) { - return -ERANGE; - - } else if (max_g > 8) { /* 16G */ - rangebits = OFFSET_LSB1_RANGE_16G; - rangescale = 1.98; - - } else if (max_g > 4) { /* 8G */ - rangebits = OFFSET_LSB1_RANGE_8G; - rangescale = 0.99; - - } else if (max_g > 3) { /* 4G */ - rangebits = OFFSET_LSB1_RANGE_4G; - rangescale = 0.5; - - } else if (max_g > 2) { /* 3G */ - rangebits = OFFSET_LSB1_RANGE_3G; - rangescale = 0.38; - - } else if (max_g > 1) { /* 2G */ - rangebits = OFFSET_LSB1_RANGE_2G; - rangescale = 0.25; - - } else { /* 1G */ - rangebits = OFFSET_LSB1_RANGE_1G; - rangescale = 0.13; + // workaround for bugged versions of MPU6k (rev C) + switch (_product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; + return OK; } - /* adjust sensor configuration */ - modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); - _range_scale = rangescale; -#endif + uint8_t afs_sel; + float lsb_per_g; + float max_accel_g; + + if (max_g_in > 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; + } + + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (MPU6000_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * MPU6000_ONE_G; + return OK; } @@ -1711,43 +1684,53 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, arb.x, arb.y, arb.z); - arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; + _last_temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature = _last_temperature; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - // apply user specified rotation - rotate_3f(_rotation, grb.x, grb.y, grb.z); - grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature = _last_temperature; _accel_reports->force(&arb); _gyro_reports->force(&grb); @@ -1795,6 +1778,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.1f\n", (double)_last_temperature); } void diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c8211b8dd1..c946e38cb8 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,4 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7b09a4676a..92afc7cd74 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -664,7 +664,7 @@ PX4FMU::task_main() } /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f62df54f6c..e5636ff0f3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -88,6 +88,7 @@ #include #include #include +#include #include @@ -288,6 +289,7 @@ private: orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety + orb_advert_t _to_mixer_status; ///< mixer status flags actuator_outputs_s _outputs; ///< mixed outputs servorail_status_s _servorail_status; ///< servorail status @@ -301,6 +303,10 @@ private: uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled + int32_t _rssi_pwm_chan; ///< RSSI PWM input channel + int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel + int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power #endif @@ -513,6 +519,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _to_mixer_status(0), _outputs{}, _servorail_status{}, _primary_pwm_device(false), @@ -521,7 +528,10 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _cb_flighttermination(true) + _cb_flighttermination(true), + _rssi_pwm_chan(0), + _rssi_pwm_max(0), + _rssi_pwm_min(0) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -661,6 +671,10 @@ PX4IO::init() if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + /* * Check for IO flight state - if FMU was flagged to be in * armed state, FMU is recovering from an in-air reset. @@ -1066,6 +1080,10 @@ PX4IO::task_main() /* Update Circuit breakers */ _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); + param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); + param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); + } } @@ -1291,6 +1309,24 @@ PX4IO::io_set_rc_config() input_map[ichan - 1] = 4; } + /* AUX 1*/ + param_get(param_find("RC_MAP_AUX1"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 5; + } + + /* AUX 2*/ + param_get(param_find("RC_MAP_AUX2"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 6; + } + + /* AUX 3*/ + param_get(param_find("RC_MAP_AUX3"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 7; + } + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { @@ -1612,6 +1648,15 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.values[i] = regs[prolog + i]; } + /* get RSSI from input channel */ + if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { + int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / + ((_rssi_pwm_max - _rssi_pwm_min) / 100); + rssi = rssi > 100 ? 100 : rssi; + rssi = rssi < 0 ? 0 : rssi; + input_rc.rssi = rssi; + } + return ret; } @@ -1669,20 +1714,28 @@ PX4IO::io_publish_pwm_outputs() { /* data we are going to fetch */ actuator_outputs_s outputs; + multirotor_motor_limits_s motor_limits; + outputs.timestamp = hrt_absolute_time(); /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - if (ret != OK) + if (ret != OK){ return ret; + } + + unsigned maxouts = sizeof(outputs.output) / sizeof(outputs.output[0]); + unsigned actuator_max = (_max_actuators > maxouts) ? maxouts : _max_actuators; + /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < actuator_max; i++){ outputs.output[i] = ctl[i]; + } - outputs.noutputs = _max_actuators; + outputs.noutputs = actuator_max; /* lazily advertise on first publication */ if (_to_outputs == 0) { @@ -1694,6 +1747,21 @@ PX4IO::io_publish_pwm_outputs() orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } + /* get mixer status flags from IO */ + uint16_t mixer_status; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status,sizeof(mixer_status)/sizeof(uint16_t)); + memcpy(&motor_limits,&mixer_status,sizeof(motor_limits)); + + if (ret != OK) + return ret; + + /* publish mixer status */ + if(_to_mixer_status == 0) { + _to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits); + } else { + orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits); + } + return OK; } @@ -1998,13 +2066,13 @@ PX4IO::print_status(bool extended_status) printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } - printf("actuators"); + printf("actuators (including S.BUS)"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); - printf("servos"); + printf("hardware servo ports"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 83086fd7c8..ea7178076b 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -92,8 +92,9 @@ static void usage() int roboclaw_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage(); + } if (!strcmp(argv[1], "start")) { diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d6599c0361..66641d6408 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -114,6 +114,7 @@ protected: virtual int probe(); private: + char _port[20]; float _min_distance; float _max_distance; work_s _work; @@ -199,8 +200,13 @@ SF0X::SF0X(const char *port) : _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) { + /* store port name */ + strncpy(_port, port, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + /* open fd */ - _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); if (_fd < 0) { warnx("FAIL: laser fd"); @@ -633,7 +639,7 @@ SF0X::cycle() /* fds initialized? */ if (_fd < 0) { /* open fd */ - _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); } /* collection phase? */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 8b68473489..a18b54981f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -890,8 +890,9 @@ tone_alarm_main(int argc, char *argv[]) if (argc > 1) { const char *argv1 = argv[1]; - if (!strcmp(argv1, "start")) - play_tune(TONE_STARTUP_TUNE); + if (!strcmp(argv1, "start")) { + play_tune(TONE_STOP_TUNE); + } if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d8b777b91b..1d590ae61c 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -112,8 +112,9 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators); +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators); /** * Control heading. @@ -128,7 +129,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att_sp The attitude setpoint. This is the output of the controller */ void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ static bool thread_should_exit = false; /**< Daemon exit flag */ @@ -137,8 +138,9 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators) +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators) { /* @@ -175,7 +177,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st } void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { /* @@ -192,6 +194,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { att_sp->roll_body = 0.6f; } @@ -285,7 +288,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN }}; + { .fd = att_sub, .events = POLLIN } + }; while (!thread_should_exit) { @@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (manual_sp_updated) /* get the RC (or otherwise user based) input */ + { orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ if (isfinite(manual_sp.z) && - (manual_sp.z >= 0.6f) && - (manual_sp.z <= 1.0f)) { + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ @@ -378,8 +384,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) thread_running = false; /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; + } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -393,8 +400,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); exit(1); @@ -410,8 +418,9 @@ usage(const char *reason) */ int ex_fixedwing_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index c2e94ff3d3..fbbd30201e 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -34,7 +34,7 @@ /* * @file params.c - * + * * Parameters for fixedwing demo */ diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 4ae8e90ec4..49ffff4467 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -34,7 +34,7 @@ /* * @file params.h - * + * * Definition of parameters for fixedwing example */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index da4ea825b0..ab67bd2532 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -81,8 +81,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -97,13 +99,12 @@ static void usage(const char *reason) */ int flow_position_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { printf("flow position estimator already running\n"); /* this is not an error */ exit(0); @@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + flow_position_estimator_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { - if (thread_running) + if (!strcmp(argv[1], "status")) { + if (thread_running) { printf("\tflow position estimator is running\n"); - else + + } else { printf("\tflow position estimator not started\n"); + } exit(0); } @@ -147,9 +148,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* rotation matrix for transformation of optical flow speed vectors */ static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, - { 1, 0, 0 }, - { 0, 0, 1 }}; // 90deg rotated - const float time_scale = powf(10.0f,-6.0f); + { 1, 0, 0 }, + { 0, 0, 1 } + }; // 90deg rotated + const float time_scale = powf(10.0f, -6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; static float global_speed[3] = {0.0f, 0.0f, 0.0f}; @@ -192,18 +194,18 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* init local position and filtered flow struct */ struct vehicle_local_position_s local_pos = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - .vx = 0.0f, - .vy = 0.0f, - .vz = 0.0f + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f }; struct filtered_bottom_flow_s filtered_flow = { - .sumx = 0.0f, - .sumy = 0.0f, - .vx = 0.0f, - .vy = 0.0f + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f }; /* advert pub messages */ @@ -224,37 +226,29 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); - while (!thread_should_exit) - { + while (!thread_should_exit) { - if (sensors_ready) - { + if (sensors_ready) { /*This runs at the rate of the sensors */ struct pollfd fds[2] = { - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = parameter_update_sub, .events = POLLIN } + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, 2, 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ // printf("[flow position estimator] no bottom flow.\n"); - } - else - { + } else { /* parameter update available? */ - if (fds[1].revents & POLLIN) - { + if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -264,8 +258,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* only if flow data changed */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { perf_begin(mc_loop_perf); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); @@ -284,46 +277,48 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> minimum sonar value 0.3m */ - if (!vehicle_liftoff) - { - if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (!vehicle_liftoff) { + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) { vehicle_liftoff = true; - } - else - { - if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + } + + } else { + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) { vehicle_liftoff = false; + } } /* calc dt between flow timestamps */ /* ignore first flow msg */ - if(time_last_flow == 0) - { + if (time_last_flow == 0) { time_last_flow = flow.timestamp; continue; } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; time_last_flow = flow.timestamp; /* only make position update if vehicle is lift off or DEBUG is activated*/ - if (vehicle_liftoff || params.debug) - { + if (vehicle_liftoff || params.debug) { /* copy flow */ if (flow.integration_timespan > 0) { flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + } else { flow_speed[0] = 0; flow_speed[1] = 0; } + flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + } speed[i] = sum; } @@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* convert to globalframe velocity * -> local position is currently not used for position control */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + speed[j] * PX4_R(att.R, i, j); + } global_speed[i] = sum; } @@ -354,9 +350,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.vy = global_speed[1]; local_pos.xy_valid = true; local_pos.v_xy_valid = true; - } - else - { + + } else { /* set speed to zero and let position as it is */ filtered_flow.vx = 0; filtered_flow.vy = 0; @@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !armed.armed) - { + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; local_pos.z_valid = false; - } - else - { + + } else { sonar_valid = true; } - if (sonar_valid || params.debug) - { + if (sonar_valid || params.debug) { /* simple lowpass sonar filtering */ /* if new value or with sonar update frequency */ - if (sonar_new != sonar_last || counter % 10 == 0) - { + if (sonar_new != sonar_last || counter % 10 == 0) { sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; sonar_last = sonar_new; } @@ -392,12 +383,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) float height_diff = sonar_new - sonar_lp; /* if over 1/2m spike follow lowpass */ - if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) - { + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) { local_pos.z = -sonar_lp; - } - else - { + + } else { local_pos.z = -sonar_new; } @@ -408,15 +397,14 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.timestamp = hrt_absolute_time(); /* publish local position */ - if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) - && isfinite(local_pos.vx) && isfinite(local_pos.vy)) - { + if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) { orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } /* publish filtered flow */ - if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) - { + if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) + && isfinite(filtered_flow.vy)) { orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); } @@ -427,9 +415,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } } - } - else - { + } else { /* sensors not ready waiting for first attitude msg */ /* polling */ @@ -440,19 +426,16 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* wait for a attitude message, check for exit condition every 5 s */ int ret = poll(fds, 1, 5000); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + + } else if (ret == 0) { /* no return value, ignore */ printf("[flow position estimator] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN){ + + } else { + if (fds[0].revents & POLLIN) { sensors_ready = true; printf("[flow position estimator] initialized.\n"); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c index ec3c3352d0..b565797874 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.c - * + * * Parameters for position estimator */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h index f9a9bb303b..3ab4e560fc 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.h - * + * * Parameters for position estimator */ diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 145cf99cc4..085ef1fed7 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -73,8 +73,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -89,13 +91,12 @@ static void usage(const char *reason) */ int matlab_csv_serial_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { warnx("already running\n"); /* this is not an error */ exit(0); @@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { + if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("stopped"); } @@ -139,7 +139,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) errx(1, "need a serial port name as argument"); } - const char* uart_name = argv[1]; + const char *uart_name = argv[1]; warnx("opening port %s", uart_name); @@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) thread_running = true; - while (!thread_should_exit) - { + while (!thread_should_exit) { /*This runs at the rate of the sensors */ struct pollfd fds[] = { - { .fd = accel0_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, ignore */ - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ warnx("no sensor data"); - } - else - { + + } else { /* accel0 update available? */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, + dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, + (int)accel0.z_raw, (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 8165b0ffce..0a66d3edca 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -40,7 +40,8 @@ #pragma once #include -class PublisherExample { +class PublisherExample +{ public: PublisherExample(); @@ -49,7 +50,7 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher * _rc_channels_pub; - px4::Publisher * _v_att_pub; - px4::Publisher * _parameter_update_pub; + px4::Publisher *_rc_channels_pub; + px4::Publisher *_v_att_pub; + px4::Publisher *_parameter_update_pub; }; diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index db9d85269b..4ff2cebfbc 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int publisher_main(int argc, char *argv[]); int publisher_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: publisher {start|stop|status}"); } @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 45d541137a..6e99939d14 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -34,7 +34,7 @@ /** * @file px4_daemon_app.c * daemon application example for PX4 autopilot - * + * * @author Example User */ @@ -71,8 +71,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); } @@ -80,14 +82,15 @@ usage(const char *reason) * The daemon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ int px4_daemon_app_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\trunning\n"); + } else { warnx("\tnot started\n"); } + exit(0); } @@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[]) exit(1); } -int px4_daemon_thread_main(int argc, char *argv[]) { +int px4_daemon_thread_main(int argc, char *argv[]) +{ warnx("[daemon] starting\n"); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 4e9f099ed7..e22c59fa22 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -75,30 +75,33 @@ int px4_simple_app_main(int argc, char *argv[]) for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); - + /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ printf("[px4_simple_app] Got no data within a second\n"); + } else if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ printf("[px4_simple_app] ERROR return value from poll(): %d\n" - , poll_ret); + , poll_ret); } + error_counter++; + } else { - + if (fds[0].revents & POLLIN) { /* obtained data for the first file descriptor */ struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); /* set att and publish this information for other apps */ att.roll = raw.accelerometer_m_s2[0]; @@ -106,6 +109,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.yaw = raw.accelerometer_m_s2[2]; orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } + /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 7ef9140980..41df96775c 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -155,7 +155,6 @@ int parameters_update(const struct param_handles *h, struct params *p) void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators) { - /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing @@ -267,18 +266,27 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ struct pollfd fds[2]; + fds[0].fd = param_sub; + fds[0].events = POLLIN; + fds[1].fd = att_sub; + fds[1].events = POLLIN; while (!thread_should_exit) { @@ -371,6 +379,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { actuators.control[i] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -403,7 +412,7 @@ usage(const char *reason) */ int rover_steering_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } @@ -421,7 +430,7 @@ int rover_steering_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, rover_steering_control_thread_main, - (argv) ? (char *const *)&argv[2] : (char *const *)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 496e1dcd8f..5a23dc80b8 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,7 +43,8 @@ using namespace px4; -void rc_channels_callback_function(const px4_rc_channels &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg) +{ PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -81,21 +82,24 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) +{ PX4_INFO("rc_channels_callback (method): [%llu]", - msg.data().timestamp_last_valid); + msg.data().timestamp_last_valid); PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } -void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) +{ PX4_INFO("vehicle_attitude_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); } -void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) +{ PX4_INFO("parameter_update_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); _p_test_float.update(); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 686fed023e..9eb5dd2a0a 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -43,7 +43,8 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg); -class SubscriberExample { +class SubscriberExample +{ public: SubscriberExample(); @@ -54,7 +55,7 @@ protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; - px4::Subscriber * _sub_rc_chan; + px4::Subscriber *_sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 6129b19acc..757e8ec521 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); int subscriber_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: subscriber {start|stop|status}"); } @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 1fe36d3950..7418ba4ef1 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -194,5 +194,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) tmp = z; z = x; x = -tmp; return; } + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + y = -y; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 917c7f830e..ef0f41c3a8 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -75,6 +75,7 @@ enum Rotation { ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, ROTATION_ROLL_270_YAW_270 = 26, + ROTATION_ROLL_180_PITCH_270 = 27, ROTATION_MAX }; @@ -111,14 +112,15 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 135 }, { 0, 90, 0 }, { 0, 270, 0 }, - {270, 0, 270 } + {270, 0, 270 }, + {180, 270, 0 } }; /** * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/eigen b/src/lib/eigen new file mode 160000 index 0000000000..e7850ed81f --- /dev/null +++ b/src/lib/eigen @@ -0,0 +1 @@ +Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 4215b49d28..3b276c5563 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -76,7 +76,7 @@ private: method is checked for further adavancing in the state machine (e.g. when to power up the motors) */ - LaunchMethod* launchMethods[1]; + LaunchMethod *launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index e16f33bd68..0ae545f1a5 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -44,9 +44,10 @@ #include "Limits.hpp" -namespace math { +namespace math +{ -#ifndef CONFIG_ARCH_ARM +#ifndef CONFIG_ARCH_ARM #define M_PI_F 3.14159265358979323846f #endif diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fca4197b8d..f04d725078 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -42,7 +42,8 @@ #include #include -namespace math { +namespace math +{ float __EXPORT min(float val1, float val2); diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 2fa2f7e7c4..c52771ab8c 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -51,7 +51,7 @@ bool __EXPORT equal(float a, float b, float epsilon) printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); return false; - } else return true; + } else { return true; } } void __EXPORT float2SigExp( @@ -84,10 +84,10 @@ void __EXPORT float2SigExp( // cheap power since it is integer if (exp > 0) { - for (int i = 0; i < abs(exp); i++) sig /= 10; + for (int i = 0; i < abs(exp); i++) { sig /= 10; } } else { - for (int i = 0; i < abs(exp); i++) sig *= 10; + for (int i = 0; i < abs(exp); i++) { sig *= 10; } } } diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h new file mode 100644 index 0000000000..2cd98e59aa --- /dev/null +++ b/src/lib/px4_eigen.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_eigen.h + * + * Compatability header to make Eigen compile on the PX4 stack + * @author Johan Jansen + */ + +#pragma once + +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include +#include +#pragma GCC diagnostic pop diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5c53a1602f..f758279035 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -104,7 +104,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, - uint16_t max_chan_count) + uint16_t max_chan_count) { int ret = 1; @@ -113,6 +113,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case ST24_DECODE_STATE_UNSYNCED: if (byte == ST24_STX1) { _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { ret = 3; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 454234601d..0cf732824b 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -158,6 +158,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ __EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index a98c986bb7..cea7790ec1 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -269,7 +269,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - *rssi = 255; + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { diff --git a/src/lib/version/version.h b/src/lib/version/version.h index d8ccb67748..b79fee182e 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -43,14 +43,6 @@ #ifndef VERSION_H_ #define VERSION_H_ -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_GIT STRINGIFY(GIT_VERSION) - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define HW_ARCH "PX4FMU_V1" #endif @@ -63,4 +55,8 @@ #define HW_ARCH "AEROCORE" #endif +#ifdef CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY +#define HW_ARCH "PX4_STM32F4DISCOVERY" +#endif + #endif /* VERSION_H_ */ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c60d70b9f8..b2a3572a7d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -118,13 +118,14 @@ usage(const char *reason) */ int attitude_estimator_ekf_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("attitude_estimator_ekf already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -176,8 +177,6 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ @@ -208,14 +207,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds }; /**< init: identity matrix */ float debugOutput[4] = { 0.0f }; - int overloadcounter = 19; /* Initialize filter */ AttitudeEKF_initialize(); - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; @@ -317,7 +312,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensors - sensor app running?"); + warnx("WARNING: Not getting sensor data - sensor app running?"); } } else { @@ -446,7 +441,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + /* check that the mag vector is > 0 */ + fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; @@ -477,20 +476,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[8] = raw.magnetometer_ga[2]; } - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ @@ -560,8 +545,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 30000) - printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 30000) { + warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); + } last_data = raw.timestamp; @@ -597,12 +583,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + if (isfinite(att.q[0]) && isfinite(att.q[1]) + && isfinite(att.q[2]) && isfinite(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { - warnx("NaN in roll/pitch/yaw estimate!"); + warnx("ERR: NaN estimate!"); } perf_end(ekf_loop_perf); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5637ec102f..fe480e12b7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -48,49 +48,49 @@ /** * Body angular rate process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); /** * Body angular acceleration process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); /** * Acceleration process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /** * Magnet field vector process noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); /** * Gyro measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); /** * Accel measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); /** * Mag measurement noise * - * @group attitude_ekf + * @group Attitude EKF estimator */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); @@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); /** * Moment of inertia matrix diagonal entry (1, 1) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); @@ -110,7 +110,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); /** * Moment of inertia matrix diagonal entry (2, 2) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); /** * Moment of inertia matrix diagonal entry (3, 3) * - * @group attitude_ekf + * @group Attitude EKF estimator * @unit kg*m^2 */ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); @@ -128,7 +128,7 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); * * If set to != 0 the moment of inertia will be used in the estimator * - * @group attitude_ekf + * @group Attitude EKF estimator * @min 0 * @max 1 */ @@ -177,6 +177,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); return OK; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 9414225ca0..82bcbc66ff 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -136,8 +136,9 @@ usage(const char *reason) */ int attitude_estimator_so3_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp new file mode 100644 index 0000000000..6bb7e5c245 --- /dev/null +++ b/src/modules/commander/PreflightCheck.cpp @@ -0,0 +1,343 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** +* @file PreflightCheck.cpp +* +* Preflight check for main system components +* +* @author Lorenz Meier +* @author Johan Jansen +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "PreflightCheck.h" + +namespace Commander +{ +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_MAG%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); + int fd = open(s, O_RDONLY); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_ACC%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + + if (dynamic) { + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + 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"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } else { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } + +out: + close(fd); + return success; +} + +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_GYRO%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } + + return false; + } + + close(fd); + return success; +} + +static bool airspeedCheck(int mavlink_fd, bool optional) +{ + bool success = true; + int ret; + int fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + success = false; + goto out; + } + + if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + // XXX do not make this fatal yet + } + +out: + close(fd); + return success; +} + +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) +{ + bool failed = false; + + /* ---- MAG ---- */ + if (checkMag) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_mag_count; i++) { + bool required = (i < max_mandatory_mag_count); + + if (!magnometerCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- ACCEL ---- */ + if (checkAcc) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_accel_count; i++) { + bool required = (i < max_mandatory_accel_count); + + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + failed = true; + } + } + } + + /* ---- GYRO ---- */ + if (checkGyro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_gyro_count; i++) { + bool required = (i < max_mandatory_gyro_count); + + if (!gyroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- BARO ---- */ + if (checkBaro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_baro_count; i++) { + bool required = (i < max_mandatory_baro_count); + + if (!baroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- AIRSPEED ---- */ + if (checkAirspeed) { + if (!airspeedCheck(mavlink_fd, true)) { + failed = true; + } + } + + /* ---- RC CALIBRATION ---- */ + if (checkRC) { + if (rc_calibration_check(mavlink_fd) != OK) { + failed = true; + } + } + + /* Report status */ + return !failed; +} + +} diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h new file mode 100644 index 0000000000..66947a3470 --- /dev/null +++ b/src/modules/commander/PreflightCheck.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PreflightCheck.h + * + * Preflight check for main system components + * + * @author Johan Jansen + */ + +#pragma once + +namespace Commander +{ +/** +* Runs a preflight check on all sensors to see if they are properly calibrated and healthy +* +* The function won't fail the test if optional sensors are not found, however, +* it will fail the test if optional sensors are found but not in working condition. +* +* @param mavlink_fd +* Mavlink output file descriptor for feedback when a sensor fails +* @param checkMag +* true if the magneteometer should be checked +* @param checkAcc +* true if the accelerometers should be checked +* @param checkGyro +* true if the gyroscopes should be checked +* @param checkBaro +* true if the barometer should be checked +* @param checkRC +* true if the Remote Controller should be checked +**/ +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false); + +const unsigned max_mandatory_gyro_count = 1; +const unsigned max_optional_gyro_count = 3; + +const unsigned max_mandatory_accel_count = 1; +const unsigned max_optional_accel_count = 3; + +const unsigned max_mandatory_mag_count = 1; +const unsigned max_optional_mag_count = 3; + +const unsigned max_mandatory_baro_count = 1; +const unsigned max_optional_baro_count = 1; + +} diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d70e050006..409c2ea003 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -120,8 +120,11 @@ * @author Anton Babushkin */ +// FIXME: Can some of these headers move out with detect_ move? + #include "accelerometer_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -149,26 +152,27 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; -static const unsigned max_sens = 3; - -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors); -int detect_orientation(int mavlink_fd, int (&subs)[max_sens]); -int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num); +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); +int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g); +int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); +int accel_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int subs[max_accel_sens]; + float accel_ref[max_accel_sens][detect_orientation_side_count][3]; +} accel_worker_data_t; int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_sens]; + int32_t device_id[max_accel_sens]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); - sleep(3); - mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); - sleep(5); - struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -183,7 +187,7 @@ int do_accel_calibration(int mavlink_fd) char str[30]; /* reset all sensors */ - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = open(str, 0); @@ -198,12 +202,12 @@ int do_accel_calibration(int mavlink_fd) close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); } } - float accel_offs[max_sens][3]; - float accel_T[max_sens][3][3]; + float accel_offs[max_accel_sens][3]; + float accel_T[max_accel_sens][3][3]; unsigned active_sensors; if (res == OK) { @@ -239,27 +243,27 @@ int do_accel_calibration(int mavlink_fd) accel_scale.y_scale = accel_T_rotated(1, 1); accel_scale.z_offset = accel_offs_rotated(2); accel_scale.z_scale = accel_T_rotated(2, 2); - + bool failed = false; /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.x_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.y_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed |= (OK != param_set(param_find(str), &(accel_scale.z_scale))); + failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", i); - failed |= (OK != param_set(param_find(str), &(device_id[i]))); + failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); if (failed) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i); return ERROR; } @@ -275,7 +279,7 @@ int do_accel_calibration(int mavlink_fd) } if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i); } } @@ -296,316 +300,116 @@ int do_accel_calibration(int mavlink_fd) return res; } -int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_sens][3], float (&accel_T)[max_sens][3][3], unsigned *active_sensors) +int accel_calibration_worker(detect_orientation_return orientation, void* data) { const unsigned samples_num = 3000; + accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); + + mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation)); + + read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num); + + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation), + (double)worker_data->accel_ref[0][orientation][0], + (double)worker_data->accel_ref[0][orientation][1], + (double)worker_data->accel_ref[0][orientation][2]); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count); + + return OK; +} + +int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) +{ + int result = OK; + *active_sensors = 0; + + accel_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; - float accel_ref[max_sens][6][3]; - bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; + bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false }; - int subs[max_sens]; + // Initialize subs to error condition so we know which ones are open and which are not + for (size_t i=0; i= 0) { + /* figure out which sensors were active */ + struct accel_report arp = {}; + (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); + if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { + (*active_sensors)++; + } + close(worker_data.subs[i]); } - close(subs[i]); } - if (res == OK) { + if (result == OK) { /* calculate offsets and transform matrix */ for (unsigned i = 0; i < (*active_sensors); i++) { - res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* verbose output on the console */ - printf("accel_T transformation matrix:\n"); - for (unsigned j = 0; j < 3; j++) { - printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); - } - printf("\n"); - - if (res != OK) { + if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; } } } - return res; -} - -/** - * Wait for vehicle become still and detect it's orientation. - * - * @param mavlink_fd the MAVLink file descriptor to print output to - * @param subs the accelerometer subscriptions. Only the first one will be used. - * - * @return 0..5 according to orientation when vehicle is still and ready for measurements, - * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -int detect_orientation(int mavlink_fd, int (&subs)[max_sens]) -{ - const unsigned ndim = 3; - - struct accel_report sensor; - /* exponential moving average of accel */ - float accel_ema[ndim] = { 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.5f; - /* set "still" threshold to 0.25 m/s^2 */ - float still_thr2 = powf(0.25f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - hrt_abstime still_time = 2000000; - struct pollfd fds[1]; - fds[0].fd = subs[0]; - fds[0].events = POLLIN; - - hrt_abstime t_start = hrt_absolute_time(); - /* set timeout to 30s */ - hrt_abstime timeout = 30000000; - hrt_abstime t_timeout = t_start + timeout; - hrt_abstime t = t_start; - hrt_abstime t_prev = t_start; - hrt_abstime t_still = 0; - - unsigned poll_errcount = 0; - - while (true) { - /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_accel), subs[0], &sensor); - t = hrt_absolute_time(); - float dt = (t - t_prev) / 1000000.0f; - t_prev = t; - float w = dt / ema_len; - - for (unsigned i = 0; i < ndim; i++) { - - float di = 0.0f; - switch (i) { - case 0: - di = sensor.x; - break; - case 1: - di = sensor.y; - break; - case 2: - di = sensor.z; - break; - } - - float d = di - accel_ema[i]; - accel_ema[i] += d * w; - d = d * d; - accel_disp[i] = accel_disp[i] * (1.0f - w); - - if (d > still_thr2 * 8.0f) { - d = still_thr2 * 8.0f; - } - - if (d > accel_disp[i]) { - accel_disp[i] = d; - } - } - - /* still detector with hysteresis */ - if (accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2) { - /* is still now */ - if (t_still == 0) { - /* first time */ - mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); - t_still = t; - t_timeout = t + timeout; - - } else { - /* still since t_still */ - if (t > t_still + still_time) { - /* vehicle is still, exit from the loop to detection of its orientation */ - break; - } - } - - } else if (accel_disp[0] > still_thr2 * 4.0f || - accel_disp[1] > still_thr2 * 4.0f || - accel_disp[2] > still_thr2 * 4.0f) { - /* not still, reset still start time */ - if (t_still != 0) { - mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); - sleep(3); - t_still = 0; - } - } - - } else if (poll_ret == 0) { - poll_errcount++; - } - - if (t > t_timeout) { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - return ERROR; - } - } - - if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 0; // [ g, 0, 0 ] - } - - if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 1; // [ -g, 0, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 2; // [ 0, g, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) { - return 3; // [ 0, -g, 0 ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { - return 4; // [ 0, 0, g ] - } - - if (fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { - return 5; // [ 0, 0, -g ] - } - - mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); - - return ERROR; // Can't detect orientation + return result; } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num) +int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - struct pollfd fds[max_sens]; + struct pollfd fds[max_accel_sens]; - for (unsigned i = 0; i < max_sens; i++) { + for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; fds[i].events = POLLIN; } - unsigned counts[max_sens] = { 0 }; - float accel_sum[max_sens][3]; + unsigned counts[max_accel_sens] = { 0 }; + float accel_sum[max_accel_sens][3]; memset(accel_sum, 0, sizeof(accel_sum)); unsigned errcount = 0; /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { - int poll_ret = poll(&fds[0], max_sens, 1000); + int poll_ret = poll(&fds[0], max_accel_sens, 1000); if (poll_ret > 0) { - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { bool changed; orb_check(subs[s], &changed); @@ -632,10 +436,9 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 } } - for (unsigned s = 0; s < max_sens; s++) { + for (unsigned s = 0; s < max_accel_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; - warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]); } } @@ -665,7 +468,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) return OK; } -int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_sens][6][3], float (&accel_T)[max_sens][3][3], float (&accel_offs)[max_sens][3], float g) +int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) { /* calculate offsets */ for (unsigned i = 0; i < 3; i++) { diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index b1e209efc0..29f44dc368 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -47,10 +47,12 @@ #define CAL_FAILED_MSG "%s calibration: failed" #define CAL_PROGRESS_MSG "%s calibration: progress <%u>" -#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" -#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" -#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" -#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error" +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u" #define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" +#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation" #endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 5796204bfd..192b8c7270 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -38,14 +38,26 @@ * @author Lorenz Meier */ +#include #include #include +#include +#include +#include +#include +#include +#include #include "calibration_routines.h" +#include "calibration_messages.h" +#include "commander_helper.h" +// FIXME: Fix return codes +static const int ERROR = -1; int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius) { float x_sumplain = 0.0f; @@ -218,3 +230,264 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } +enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) +{ + const unsigned ndim = 3; + + struct accel_report sensor; + /* exponential moving average of accel */ + float accel_ema[ndim] = { 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.5f; + /* set "still" threshold to 0.25 m/s^2 */ + float still_thr2 = powf(0.25f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + hrt_abstime still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = accel_sub; + fds[0].events = POLLIN; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + + for (unsigned i = 0; i < ndim; i++) { + + float di = 0.0f; + switch (i) { + case 0: + di = sensor.x; + break; + case 1: + di = sensor.y; + break; + case 2: + di = sensor.z; + break; + } + + float d = di - accel_ema[i]; + accel_ema[i] += d * w; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + + if (d > still_thr2 * 8.0f) { + d = still_thr2 * 8.0f; + } + + if (d > accel_disp[i]) { + accel_disp[i] = d; + } + } + + /* still detector with hysteresis */ + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still..."); + t_still = t; + t_timeout = t + timeout; + + } else { + /* still since t_still */ + if (t > t_still + still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); + t_still = 0; + } + } + + } else if (poll_ret == 0) { + poll_errcount++; + } + + if (t > t_timeout) { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return DETECT_ORIENTATION_ERROR; + } + } + + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ] + } + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) { + return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ] + } + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ] + } + + mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation"); + + return DETECT_ORIENTATION_ERROR; // Can't detect orientation +} + +const char* detect_orientation_str(enum detect_orientation_return orientation) +{ + static const char* rgOrientationStrs[] = { + "back", // tail down + "front", // nose down + "left", + "right", + "up", // upside-down + "down", // right-side up + "error" + }; + + return rgOrientationStrs[orientation]; +} + +int calibrate_from_orientation(int mavlink_fd, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data) +{ + int result = OK; + + // Setup subscriptions to onboard accel sensor + + int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + if (sub_accel < 0) { + mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort"); + return ERROR; + } + + unsigned orientation_failures = 0; + + // Rotate through all three main positions + while (true) { + if (orientation_failures > 10) { + result = ERROR; + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT); + break; + } + + unsigned int side_complete_count = 0; + + // Update the number of completed sides + for (unsigned i = 0; i < detect_orientation_side_count; i++) { + if (side_data_collected[i]) { + side_complete_count++; + } + } + + if (side_complete_count == detect_orientation_side_count) { + // We have completed all sides, move on + break; + } + + /* inform user which orientations are still needed */ + char pendingStr[256]; + pendingStr[0] = 0; + + for (unsigned int cur_orientation=0; cur_orientation= 0) { + close(sub_accel); + } + + // FIXME: Do we need an orientation complete routine? + + return result; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3c8e49ee9e..8f34f02044 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -57,4 +57,48 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius); + +// FIXME: Change the name +static const unsigned max_accel_sens = 3; + +// The order of these cannot change since the calibration calculations depend on them in this order +enum detect_orientation_return { + DETECT_ORIENTATION_TAIL_DOWN, + DETECT_ORIENTATION_NOSE_DOWN, + DETECT_ORIENTATION_LEFT, + DETECT_ORIENTATION_RIGHT, + DETECT_ORIENTATION_UPSIDE_DOWN, + DETECT_ORIENTATION_RIGHTSIDE_UP, + DETECT_ORIENTATION_ERROR +}; +static const unsigned detect_orientation_side_count = 6; + +/** + * Wait for vehicle to become still and detect it's orientation. + * + * @param mavlink_fd the MAVLink file descriptor to print output to + * @param accel_sub Subscription to onboard accel + * + * @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements, + * DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub); + + +/** + * Returns the human readable string representation of the orientation + * + * @param orientation Orientation to return string for, "error" if buffer is too small + * + * @return str Returned orientation string + */ +const char* detect_orientation_str(enum detect_orientation_return orientation); + +typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data); + +int calibrate_from_orientation(int mavlink_fd, + bool side_data_collected[detect_orientation_side_count], + calibration_from_orientation_worker_t calibration_worker, + void* worker_data); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 710ed85a19..65fc8f90e8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -114,6 +114,7 @@ #include "baro_calibration.h" #include "rc_calibration.h" #include "airspeed_calibration.h" +#include "PreflightCheck.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -156,20 +157,19 @@ enum MAV_MODE_FLAG { /* Mavlink file descriptors */ static int mavlink_fd = 0; -/* Syste autostart ID */ +/* System autostart ID */ static int autostart_id; /* flags */ static bool commander_initialized = false; -static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ +static int daemon_task; /**< Handle of daemon task / thread */ +static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -/* if connected via USB */ -static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; @@ -262,7 +262,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } @@ -278,7 +278,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3200, + 3400, commander_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); @@ -379,8 +379,8 @@ void usage(const char *reason) void print_status() { - warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); - warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); + warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ @@ -536,9 +536,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); @@ -550,6 +550,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } + else { + + //Refuse to arm if preflight checks have failed + if(!status.condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); + cmd_result = VEHICLE_CMD_RESULT_DENIED; + break; + } + + } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); @@ -727,6 +737,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_CUSTOM_2: case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case VEHICLE_CMD_DO_MOUNT_CONTROL: + case VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: + case VEHICLE_CMD_DO_MOUNT_CONFIGURE: /* ignore commands that handled in low prio loop */ break; @@ -820,6 +833,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); + param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -861,12 +875,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_t commander_low_prio_thread; /* initialize */ - if (led_init() != 0) { - warnx("ERROR: LED INIT FAIL"); + if (led_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: BUZZER INIT FAIL"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BUZZER INIT FAIL"); + } + + if (battery_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -903,6 +921,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + status.usb_connected = false; // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; @@ -964,19 +983,6 @@ int commander_thread_main(int argc, char *argv[]) int ret; - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - pthread_attr_destroy(&commander_low_prio_attr); - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1080,8 +1086,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to parameters changed topic */ int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -1121,6 +1125,26 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; + /* update vehicle status to find out vehicle type (required for preflight checks) */ + param_get(_param_sys_type, &(status.system_type)); // get system type + status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); + + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + + //Run preflight check + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + if(!status.condition_system_sensors_initialized) { + set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune + } + else { + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1136,11 +1160,27 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + /* initialize low priority thread */ + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2100); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -1156,7 +1196,9 @@ int commander_thread_main(int argc, char *argv[]) if (updated || param_init_forced) { param_init_forced = false; + /* parameters changed */ + struct parameter_update_s param_changed; orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ @@ -1166,15 +1208,7 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || - status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || - status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { - + if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; } else { @@ -1182,8 +1216,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = is_vtol(&status); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1219,6 +1252,15 @@ int commander_thread_main(int argc, char *argv[]) /* Autostart id */ param_get(_param_autostart_id, &autostart_id); + + /* Parameter autosave setting */ + param_get(_param_autosave_params, &autosave_params); + } + + /* Set flag to autosave parameters if necessary */ + if (updated && autosave_params != 0) { + /* trigger an autosave */ + need_param_autosave = true; } orb_check(sp_man_sub, &updated); @@ -1262,7 +1304,15 @@ int commander_thread_main(int argc, char *argv[]) telemetry.heartbeat_time > 0 && hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { - (void)rc_calibration_check(mavlink_fd); + bool chAirspeed = 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) { + chAirspeed = true; + } + + /* provide RC and sensor status feedback to the user */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1320,6 +1370,7 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + status.usb_connected = system_power.usb_connected; } } @@ -1366,8 +1417,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { + /* Make sure that this is only adjusted if vehicle really is of type vtol*/ + if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1395,14 +1446,14 @@ int commander_thread_main(int argc, char *argv[]) if(status.condition_global_position_valid) { set_tune_override(TONE_GPS_WARNING_TUNE); status_changed = true; - status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } } else if(global_position.timestamp != 0) { //Got good global position estimate if(!status.condition_global_position_valid) { status_changed = true; - status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } } @@ -1523,10 +1574,6 @@ int commander_thread_main(int argc, char *argv[]) } last_idle_time = system_load.tasks[0].total_runtime; - - /* check if board is connected via USB */ - struct stat statbuf; - on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1536,28 +1583,25 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + } else if (!status.usb_connected && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; - if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, - mavlink_fd); + if (!armed.armed) { + arming_ret = arming_state_transition(&status, &safety, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN"); } } else { - arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, - mavlink_fd); - - if (arming_ret == TRANSITION_CHANGED) { - arming_state_changed = true; - } + mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); } status_changed = true; @@ -2093,7 +2137,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); @@ -2175,13 +2219,34 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + /* mode rejected, continue to evaluate the main system mode */ } else { + /* changed successfully or already in this state */ return res; } } - /* offboard switched off or denied, check main mode switch */ + /* RTL switch overrides main switch */ + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); + + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "AUTO_RTL"); + + /* fallback to LOITER if home position not set */ + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); + } + + if (res != TRANSITION_DENIED) { + /* changed successfully or already in this state */ + return res; + } + + /* if we get here mode was rejected, continue to evaluate the main system mode */ + } + + /* offboard and RTL switches off or denied, check main mode switch */ switch (sp_man->mode_switch) { case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -2226,23 +2291,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode(status_local, "AUTO_RTL"); - - // fallback to LOITER if home position not set - res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { @@ -2528,6 +2577,9 @@ void *commander_low_prio_loop(void *arg) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* timeout for param autosave */ + hrt_abstime need_param_autosave_timeout = 0; + /* wakeup source(s) */ struct pollfd fds[1]; @@ -2536,197 +2588,239 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - /* wait for up to 200ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + /* wait for up to 1000ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - /* if we reach here, we have a valid command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* ignore commands the high-prio loop handles */ - if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) { - continue; - } - - /* only handle low-priority commands here */ - switch (cmd.command) { - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { - - if (((int)(cmd.param1)) == 1) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(100000); - /* reboot */ - systemreset(false); - - } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(100000); - /* reboot to bootloader */ - systemreset(true); - - } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } - - } else { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } - - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - - int calib_ret = ERROR; - - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, - true /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - break; - } - - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); - - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); - - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - /* disable RC control input completely */ - status.rc_input_blocked = true; - calib_ret = OK; - mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); - - } else if ((int)(cmd.param4) == 2) { - /* RC trim calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_trim_calibration(mavlink_fd); - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); - - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - - } else if ((int)(cmd.param4) == 0) { - /* RC calibration ended - have we been in one worth confirming? */ - if (status.rc_input_blocked) { - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - /* enable RC control input */ - status.rc_input_blocked = false; - mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - } - - /* this always succeeds */ - calib_ret = OK; - - } - - if (calib_ret == OK) { - tune_positive(true); - - } else { - tune_negative(true); - } - - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - - if (((int)(cmd.param1)) == 0) { - int ret = param_load_default(); - - if (ret == OK) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); - } - - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } - - } else if (((int)(cmd.param1)) == 1) { + /* trigger a param autosave if required */ + if (need_param_autosave) { + if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); if (ret == OK) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); } else { - mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); - - /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) { - ret = -ret; - } - - if (ret < 1000) { - mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); - } - - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + mavlink_and_console_log_critical(mavlink_fd, "settings save error"); } + + need_param_autosave = false; + need_param_autosave_timeout = 0; + } else { + need_param_autosave_timeout = hrt_absolute_time(); + } + } + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ + warn("poll error %d, %d", pret, errno); + continue; + } else { + + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { + continue; + } + + /* only handle low-priority commands here */ + switch (cmd.command) { + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot */ + systemreset(false); + + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot to bootloader */ + systemreset(true); + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + int calib_ret = ERROR; + + /* try to go to INIT/PREFLIGHT arming state */ + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* disable RC control input completely */ + status.rc_input_blocked = true; + calib_ret = OK; + mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_trim_calibration(mavlink_fd); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status.rc_input_blocked) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* enable RC control input */ + status.rc_input_blocked = false; + mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + } + + /* this always succeeds */ + calib_ret = OK; + + } + + if (calib_ret == OK) { + tune_positive(true); + + } else { + tune_negative(true); + } + + // Update preflight check status + // we do not set the calibration return value based on it because the calibration + // might have worked just fine, but the preflight check fails for a different reason, + // so this would be prone to false negatives. + + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + + break; + } + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + + bool checkAirspeed = false; + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { + checkAirspeed = true; + } + + // Update preflight check status + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + + if (((int)(cmd.param1)) == 0) { + int ret = param_load_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "settings load ERROR"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } + + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + + } else if (((int)(cmd.param1)) == 1) { + int ret = param_save_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "settings saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "settings save error"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) { + ret = -ret; + } + + if (ret < 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + } + + break; + } + + case VEHICLE_CMD_START_RX_PAIR: + /* handled in the IO driver */ + break; + + default: + /* don't answer on unsupported commands, it will be done in main loop */ + break; } - case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; - - default: - /* don't answer on unsupported commands, it will be done in main loop */ - break; - } - - /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { - /* send acknowledge command */ - // XXX TODO + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b5ec6c4e6f..5f735b7b7c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -84,12 +84,40 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } +bool is_vtol(const struct vehicle_status_s * current_status) { + return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR || + current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR; +} + static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; +static param_t bat_v_empty_h; +static param_t bat_v_full_h; +static param_t bat_n_cells_h; +static param_t bat_capacity_h; +static param_t bat_v_load_drop_h; +static float bat_v_empty = 3.4f; +static float bat_v_full = 4.2f; +static float bat_v_load_drop = 0.06f; +static int bat_n_cells = 3; +static float bat_capacity = -1.0f; +static unsigned int counter = 0; + +int battery_init() +{ + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_CHARGED"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); + + return OK; +} + int buzzer_init() { tune_end = 0; @@ -231,7 +259,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED0_DEVICE_PATH, 0); - if (rgbleds == -1) { + if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); } @@ -240,76 +268,69 @@ int led_init() void led_deinit() { - close(leds); + if (leds >= 0) { + close(leds); + } - if (rgbleds != -1) { + if (rgbleds >= 0) { close(rgbleds); } } int led_toggle(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_ON, led); } int led_off(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; - static param_t bat_v_empty_h; - static param_t bat_v_full_h; - static param_t bat_n_cells_h; - static param_t bat_capacity_h; - static param_t bat_v_load_drop_h; - static float bat_v_empty = 3.4f; - static float bat_v_full = 4.2f; - static float bat_v_load_drop = 0.06f; - static int bat_n_cells = 3; - static float bat_capacity = -1.0f; - static bool initialized = false; - static unsigned int counter = 0; - - if (!initialized) { - bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_CHARGED"); - bat_n_cells_h = param_find("BAT_N_CELLS"); - bat_capacity_h = param_find("BAT_CAPACITY"); - bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); - initialized = true; - } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index cd3db73247..bf0c0505d2 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -51,6 +51,7 @@ bool is_multirotor(const struct vehicle_status_s *current_status); bool is_rotary_wing(const struct vehicle_status_s *current_status); +bool is_vtol(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); @@ -73,6 +74,8 @@ void rgbled_set_color(rgbled_color_t color); void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_pattern(rgbled_pattern_t *pattern); +int battery_init(); + /** * Estimate remaining battery charge. * diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1b0c4258b2..6663525cc4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * Defines the voltage where a single cell of the battery is considered empty. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -64,6 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * Defines the voltage where a single cell of the battery is considered full. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -74,6 +76,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * to maximum current ratio and assumes linearity. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -83,6 +86,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * Defines the number of cells the attached battery consists of. * * @group Battery Calibration + * @unit S */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -92,6 +96,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * Defines the capacity of the attached battery. * * @group Battery Calibration + * @unit mA */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); @@ -100,7 +105,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * * Set to 1 to enable actions triggered when the datalink is lost. * - * @group commander + * @group Commander * @min 0 * @max 1 */ @@ -110,7 +115,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); * * After this amount of seconds without datalink the data link lost mode triggers * - * @group commander + * @group Commander * @unit second * @min 0 * @max 30 @@ -122,7 +127,7 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' * flag is set back to false * - * @group commander + * @group Commander * @unit second * @min 0 * @max 30 @@ -133,9 +138,9 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * * Engine failure triggers only above this throttle value * - * @group commander - * @min 0.0f - * @max 1.0f + * @group Commander + * @min 0.0 + * @max 1.0 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -143,9 +148,9 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * * Engine failure triggers only below this current/throttle value * - * @group commander - * @min 0.0f - * @max 7.0f + * @group Commander + * @min 0.0 + * @max 7.0 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -154,10 +159,10 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * Engine failure triggers only if the throttle threshold and the * current to throttle threshold are violated for this time * - * @group commander + * @group Commander * @unit second - * @min 0.0f - * @max 7.0f + * @min 0.0 + * @max 7.0 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); @@ -165,9 +170,21 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * * After this amount of seconds without RC connection the rc lost flag is set to true * - * @group commander + * @group Commander * @unit second * @min 0 * @max 35 */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); + +/** Autosaving of params + * + * If not equal to zero the commander will automatically save parameters to persistent storage once changed. + * Default is on, as the interoperability with currently deployed GCS solutions depends on parameters + * being sticky. Developers can default it to off. + * + * @group Commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk index 4d10275d1e..64afa86c44 100644 --- a/src/modules/commander/commander_tests/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = commander_tests SRCS = commander_tests.cpp \ state_machine_helper_test.cpp \ - ../state_machine_helper.cpp + ../state_machine_helper.cpp \ + ../PreflightCheck.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 4ddb4e0fbf..967304cb45 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -255,10 +255,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Sensor tests - { "no transition: init to standby - sensors not initialized", + { "transition to standby error: init to standby - sensors not initialized", { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, // Safety switch arming tests diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dfd1657c5b..291ccfe804 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -73,31 +73,25 @@ int do_gyro_calibration(int mavlink_fd) /* wait for the user to respond */ sleep(2); - struct gyro_scale gyro_scale[max_gyros] = { { + struct gyro_scale gyro_scale_zero = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } }; + struct gyro_scale gyro_scale[max_gyros] = {}; + int res = OK; - /* store board ID */ - uint32_t mcu_id[3]; - mcu_unique_id(&mcu_id[0]); - - /* store last 32bit number - not unique, but unique in a given set */ - (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); - char str[30]; for (unsigned s = 0; s < max_gyros; s++) { /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0])); + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ @@ -109,17 +103,19 @@ int do_gyro_calibration(int mavlink_fd) device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); } } unsigned calibration_counter[max_gyros] = { 0 }; const unsigned calibration_count = 5000; + struct gyro_report gyro_report_0 = {}; + if (res == OK) { /* determine gyro mean values */ unsigned poll_errcount = 0; @@ -140,7 +136,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -150,6 +146,11 @@ int do_gyro_calibration(int mavlink_fd) if (changed) { orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + gyro_scale[s].x_offset += gyro_report.x; gyro_scale[s].y_offset += gyro_report.y; gyro_scale[s].z_offset += gyro_report.z; @@ -183,8 +184,20 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* check offsets */ - if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { - mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.0055f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration"); res = ERROR; } } @@ -201,19 +214,20 @@ int do_gyro_calibration(int mavlink_fd) } (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset))); + failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set(param_find(str), &(device_id[s]))); + failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s]))); /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = open(str, 0); if (fd < 0) { + failed = true; continue; } @@ -224,138 +238,19 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); } } - + if (failed) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } -#if 0 - /* beep on offset calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done"); - tune_neutral(); + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); - /* scale calibration */ - /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ - - mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - - /* apply new offsets */ - fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { - warn("WARNING: failed to apply new offsets for gyro"); - } - - close(fd); - - - unsigned rotations_count = 30; - float gyro_integral = 0.0f; - float baseline_integral = 0.0f; - - // XXX change to mag topic - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - - if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } - - - uint64_t last_time = hrt_absolute_time(); - uint64_t start_time = hrt_absolute_time(); - - while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { - - /* abort this loop if not rotated more than 180 degrees within 5 seconds */ - if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); - close(sub_sensor_combined); - return OK; - } - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - - float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; - last_time = hrt_absolute_time(); - - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - // XXX this is just a proof of concept and needs world / body - // transformation and more - - //math::Vector2f magNav(raw.magnetometer_ga); - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - - if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - - if (mag < -M_PI_F) { mag += 2 * M_PI_F; } - - float diff = mag - mag_last; - - if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - - if (diff < -M_PI_F) { diff += 2 * M_PI_F; } - - baseline_integral += diff; - mag_last = mag; - // Jump through some timing scale hoops to avoid - // operating near the 1e6/1e8 max sane resolution of float. - gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - -// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; - } - } - - float gyro_scale = baseline_integral / gyro_integral; - - warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); - - - if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); - close(sub_sensor_gyro); - mavlink_log_critical(mavlink_fd, "gyro calibration failed"); - return ERROR; - } - - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done"); - tune_neutral(); - - if (res == OK) { - /* set scale parameters to new values */ - if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); - res = ERROR; - } - } - - #endif + /* store last 32bit number - not unique, but unique in a given set */ + (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); if (res == OK) { /* auto-save to EEPROM */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e0786db79d..198acb0272 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -62,287 +63,406 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; +static const unsigned max_mags = 3; + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); +int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int sub_mag[max_mags]; + unsigned int calibration_points_perside; + unsigned int calibration_interval_perside_seconds; + uint64_t calibration_interval_perside_useconds; + unsigned int calibration_counter_total; + bool side_data_collected[detect_orientation_side_count]; + float* x[max_mags]; + float* y[max_mags]; + float* z[max_mags]; +} mag_worker_data_t; -int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id); int do_mag_calibration(int mavlink_fd) { - const unsigned max_mags = 3; - - int32_t device_id[max_mags]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - sleep(1); - struct mag_scale mscale_null[max_mags] = { - { + struct mag_scale mscale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } - } ; + }; - int res = ERROR; + int result = OK; + + // Determine which mags are available and reset each + int32_t device_ids[max_mags]; char str[30]; - unsigned calibrated_ok = 0; + for (size_t i=0; i(malloc(sizeof(float) * calibration_maxcount)); - y = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); - z = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); - - if (x == NULL || y == NULL || z == NULL) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); - - /* clean up */ - if (x != NULL) { - free(x); + mag_worker_data_t* worker_data = (mag_worker_data_t*)(data); + + mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + sleep(2); + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + unsigned poll_errcount = 0; + + calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + // Wait clocking for new data on all mags + struct pollfd fds[max_mags]; + size_t fd_count = 0; + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + fds[fd_count].fd = worker_data->sub_mag[cur_mag]; + fds[fd_count].events = POLLIN; + fd_count++; + } } + int poll_ret = poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + struct mag_report mag; - if (y != NULL) { - free(y); + orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; + + } + } + + worker_data->calibration_counter_total++; + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "%s %s side calibration: progress <%u>", + sensor_name, + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } else { + poll_errcount++; } - - if (z != NULL) { - free(z); + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = ERROR; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + break; } + } + + // Mark the opposite side as collected as well. No need to collect opposite side since it + // would generate similar points. + detect_orientation_return alternateOrientation = orientation; + switch (orientation) { + case DETECT_ORIENTATION_TAIL_DOWN: + alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; + break; + case DETECT_ORIENTATION_NOSE_DOWN: + alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; + break; + case DETECT_ORIENTATION_LEFT: + alternateOrientation = DETECT_ORIENTATION_RIGHT; + break; + case DETECT_ORIENTATION_RIGHT: + alternateOrientation = DETECT_ORIENTATION_LEFT; + break; + case DETECT_ORIENTATION_UPSIDE_DOWN: + alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; + break; + case DETECT_ORIENTATION_RIGHTSIDE_UP: + alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; + break; + case DETECT_ORIENTATION_ERROR: + warnx("Invalid orientation in mag_calibration_worker"); + break; + } + worker_data->side_data_collected[alternateOrientation] = true; + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); + + return result; +} - res = ERROR; - return res; +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +{ + int result = OK; + + mag_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; + worker_data.calibration_counter_total = 0; + worker_data.calibration_points_perside = 80; + worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; + + // Initialize to collect all sides + for (size_t cur_side=0; cur_side<6; cur_side++) { + worker_data.side_data_collected[cur_side] = false; + } + + for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); + result = ERROR; + } + } - if (sub_mag < 0) { - mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort"); - res = ERROR; - } else { - struct mag_report mag; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; - - mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - - calibration_counter = 0U; - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - calibration_counter++; - - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); - } - - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; + + // Setup subscriptions to mag sensors + if (result == OK) { + for (unsigned cur_mag=0; cur_mag (calibration_maxcount / 2)) { - - /* sphere fit */ - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - - if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); - res = ERROR; - } - } - - if (x != NULL) { - free(x); - } - - if (y != NULL) { - free(y); - } - - if (z != NULL) { - free(z); - } - - if (res == OK) { - /* apply calibration and set parameters */ - struct mag_scale mscale; - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = open(str, 0); - res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); - } - - if (res == OK) { - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + + // Limit update rate to get equally spaced measurements over time (in ms) + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + close(worker_data.sub_mag[cur_mag]); + } + } + + // Calculate calibration values for each mag + + + float sphere_x[max_mags]; + float sphere_y[max_mags]; + float sphere_z[max_mags]; + float sphere_radius[max_mags]; + + // Sphere fit the data to get calibration values + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + close(fd_mag); + } + + if (result == OK) { + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); + + if (failed) { + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag); + result = ERROR; + } else { + mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga", + cur_mag, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f", + cur_mag, + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + } + } + } + } + } + + return result; } diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4a1369aba9..4437041e26 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -46,11 +46,12 @@ SRCS = commander.cpp \ mag_calibration.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ - airspeed_calibration.cpp + airspeed_calibration.cpp \ + PreflightCheck.cpp MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=2000 +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0154f235f6..7410baca3e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,18 +52,17 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include "state_machine_helper.h" #include "commander_helper.h" +#include "PreflightCheck.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -77,12 +76,12 @@ static const int ERROR = -1; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; @@ -212,9 +211,21 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; valid_transition = false; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + } + + /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ + if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + } else { + mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + } + feedback_provided = true; } // Finish up the state transition @@ -649,69 +660,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - int ret; - bool failed = false; - - int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); - failed = true; - goto system_eval; - } - - ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); - failed = true; - goto system_eval; - } - - /* check measurement result range */ - struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - 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_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - } else { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - + /* at this point this equals the preflight check, but might add additional + * quantities later. + */ + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - /* accel done, close it */ - close(fd); - fd = orb_subscribe(ORB_ID(airspeed)); - - struct airspeed_s airspeed; - - if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || - (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); - failed = true; - goto system_eval; - } - - if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); - // XXX do not make this fatal yet - } + checkAirspeed = true; } -system_eval: - close(fd); - return (failed); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); } diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index a64d0139e3..437e43bfb4 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -58,7 +58,7 @@ public: * * @param parent_prefix Set to true to include the parent name in the parameter name */ - BlockParamBase(Block *parent, const char *name, bool parent_prefix=true); + BlockParamBase(Block *parent, const char *name, bool parent_prefix = true); virtual ~BlockParamBase() {}; virtual void update() = 0; const char *getName() { return param_name(_handle); } @@ -75,7 +75,7 @@ class BlockParam : public BlockParamBase { public: BlockParam(Block *block, const char *name, - bool parent_prefix=true); + bool parent_prefix = true); T get(); void set(T val); void update(); diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index bffc355a82..65600190b2 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -114,7 +114,7 @@ public: // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */), + _state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {}; virtual ~BlockLowPass() {}; @@ -492,8 +492,9 @@ public: X = V1 * float(sqrt(-2 * float(log(S)) / S)); - } else + } else { X = V2 * float(sqrt(-2 * float(log(S)) / S)); + } phase = 1 - phase; return X * getStdDev() + getMean(); diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ec9efe8eea..4d5e56a961 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -174,7 +174,6 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -194,6 +193,7 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; + float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; @@ -208,7 +208,6 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - bool _newDataGps; bool _newHgtData; bool _newAdsData; bool _newDataMag; 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 3bb395a870..e0b4cb2a0d 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 @@ -132,72 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _wind_pub(-1), _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), - _sensor_combined {}, + _sensor_combined {}, - _pos_ref {}, - _baro_ref(0.0f), - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), + _pos_ref{}, + _baro_ref_offset(0.0f), + _baro_gps_offset(0.0f), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _baroAltRef(0.0f), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), + _distance_last_valid(0), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _gyro_main(0), + _accel_main(0), + _mag_main(0), + _ekf_logging(true), + _debug(0), - _newDataGps(false), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), - _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _mavlink_fd(-1), + _parameters {}, + _parameter_handles {}, + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -636,24 +635,26 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gpsIsGood) { - initializeGPS(); - - } else if (!_ekf->statesInitialised) { + if (!_ekf->statesInitialised) { // North, East Down position (m) float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; - _local_pos.ref_alt = _baro_ref; + _local_pos.ref_alt = 0.0f; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } else if (_ekf->statesInitialised) { + } else { + + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + continue; + } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); @@ -668,7 +669,7 @@ void AttitudePositionEstimatorEKF::task_main() } //Run EKF data fusion steps - updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -717,7 +718,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + _ekf->hgtMea = _ekf->baroHgt; // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; @@ -783,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; - _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; - _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; + _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -858,7 +859,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -908,8 +909,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, - const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) + const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed) { // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -937,7 +937,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // Convert GPS measurements to Pos NE, hgt and Vel NED // set fusion flags - _ekf->fuseVelData = true; + _ekf->fuseVelData = _gps.vel_ned_valid; _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays @@ -978,7 +978,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseBaro) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->hgtMea = _ekf->baroHgt; _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays @@ -1069,9 +1069,9 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, + printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); @@ -1268,10 +1268,10 @@ void AttitudePositionEstimatorEKF::pollData() } - orb_check(_gps_sub, &_newDataGps); - - if (_newDataGps) { + bool gps_update; + orb_check(_gps_sub, &gps_update); + if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1324,10 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); @@ -1353,9 +1350,6 @@ void AttitudePositionEstimatorEKF::pollData() _previousGPSTimestamp = _gps.timestamp_position; - } else { - //Too poor GPS fix to use - _newDataGps = false; } } @@ -1406,21 +1400,17 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + if(!_baro_init) { + _baro_init = true; + _baroAltRef = _baro.altitude; + } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } - perf_count(_perf_baro); - - _newHgtData = true; } //Update Magnetometer @@ -1432,30 +1422,41 @@ void AttitudePositionEstimatorEKF::pollData() int last_mag_main = _mag_main; - // XXX we compensate the offsets upfront - should be close to zero. + Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], + _sensor_combined.magnetometer_ga[2]); + + Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], + _sensor_combined.magnetometer1_ga[2]); + + const unsigned mag_timeout_us = 200000; /* fail over to the 2nd mag if we know the first is down */ - if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) { - _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && + _sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount && + mag0.length() > 0.1f) { + _ekf->magData.x = mag0.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magData.y = mag0.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magData.z = mag0.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 0; - } else { - _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; + } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && + mag1.length() > 0.1f) { + _ekf->magData.x = mag1.x; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - _ekf->magData.y = _sensor_combined.magnetometer1_ga[1]; + _ekf->magData.y = mag1.y; _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - _ekf->magData.z = _sensor_combined.magnetometer1_ga[2]; + _ekf->magData.z = mag1.z; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 1; + } else { + _mag_valid = false; } if (last_mag_main != _mag_main) { @@ -1526,7 +1527,7 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 8c82dd6c14..f8cca6c0dd 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); /** * Accelerometer bias estimate process noise @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); /** * Magnetometer earth frame offsets process noise diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5c01286e30..967402fff7 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -41,13 +41,17 @@ #include #include #include -#include +#include #include #ifndef M_PI_F #define M_PI_F static_cast(M_PI) #endif +#ifndef isfinite +#define isfinite(__x) std::isfinite(__x) +#endif + constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : @@ -210,10 +214,10 @@ void AttPosEKF::InitialiseParameters() yawVarScale = 1.0f; windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; + dAngBiasSigma = 1.0e-6; + dVelBiasSigma = 0.0002f; + magEarthSigma = 0.0003f; + magBodySigma = 0.0003f; vneSigma = 0.2f; vdSigma = 0.3f; @@ -414,9 +418,10 @@ void AttPosEKF::CovariancePrediction(float dt) // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + float gyroBiasScale = (_onGround) ? 2.0f : 1.0f; + + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale; processNoise[13] = dVelBiasSigma; if (!inhibitWindStates) { for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; @@ -2329,15 +2334,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (size_t i=0; i= EKF_DATA_BUFFER_SIZE) { storeIndex = 0; + } } void AttPosEKF::ResetStoredStates() @@ -2350,10 +2360,8 @@ void AttPosEKF::ResetStoredStates() // reset store index to first storeIndex = 0; - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; + //Reset stored state to current state + StoreStates(millis()); } // Output the state vector stored at the time that best matches that specified by msec @@ -2513,27 +2521,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) -{ - posNED[0] = earthRadius * (lat - latReference); - posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); - posNED[2] = -(hgt - hgtReference); -} - -void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) -{ - lat = latRef + (double)posNED[0] * earthRadiusInv; - lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - void AttPosEKF::setOnGround(const bool isLanded) { _onGround = isLanded; @@ -2592,25 +2579,40 @@ void AttPosEKF::CovarianceInit() P[1][1] = 0.25f * sq(1.0f*deg2rad); P[2][2] = 0.25f * sq(1.0f*deg2rad); P[3][3] = 0.25f * sq(10.0f*deg2rad); + + //velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; P[6][6] = sq(0.7f); + + //positions P[7][7] = sq(15.0f); P[8][8] = P[7][7]; P[9][9] = sq(5.0f); + + //delta angle biases P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; + + //Z delta velocity bias P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(0.0f); + + //Wind velocities + P[14][14] = 0.0f; P[15][15] = P[14][14]; + + //Earth magnetic field P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; + + //Body magnetic field P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; + //Optical flow fScaleFactorVar = 0.001f; // focal length scale factor variance Popt[0][0] = 0.001f; } @@ -2629,7 +2631,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) } if (!isfinite(val)) { - //ekf_debug("constrain: non-finite!"); + ekf_debug("constrain: non-finite!"); } return ret; @@ -2707,6 +2709,11 @@ void AttPosEKF::ConstrainStates() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // Constrain dtIMUfilt + if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { + dtIMUfilt = dtIMU; + } + // Constrain quaternion for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); @@ -2728,11 +2735,11 @@ void AttPosEKF::ConstrainStates() // Angle bias limit - set to 8 degrees / sec for (size_t i = 10; i <= 12; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt); } // Constrain delta velocity bias - states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt); // Wind velocity limits - assume 120 m/s max velocity for (size_t i = 14; i <= 15; i++) { @@ -2796,8 +2803,8 @@ bool AttPosEKF::GyroOffsetsDiverged() // Protect against division by zero if (delta_len > 0.0f) { - float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); - delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt; } bool diverged = (delta_len_scaled > 1.0f); @@ -2863,8 +2870,12 @@ void AttPosEKF::ResetPosition(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[7][i] = states[7]; storedStates[8][i] = states[8]; - } + } } + + //reset position covariance + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; } void AttPosEKF::ResetHeight(void) @@ -2876,6 +2887,10 @@ void AttPosEKF::ResetHeight(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[9][i] = states[9]; } + + //reset altitude covariance + P[9][9] = sq(5.0f); + P[6][6] = sq(0.7f); } void AttPosEKF::ResetVelocity(void) @@ -2884,7 +2899,8 @@ void AttPosEKF::ResetVelocity(void) states[4] = 0.0f; states[5] = 0.0f; states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { + } + else if (GPSstatus >= GPS_FIX_3D) { //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading @@ -2894,8 +2910,12 @@ void AttPosEKF::ResetVelocity(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[4][i] = states[4]; storedStates[5][i] = states[5]; - } + } } + + //reset velocities covariance + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; } bool AttPosEKF::StatesNaN() { @@ -3012,10 +3032,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -3029,10 +3049,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); ret = 0; } @@ -3202,10 +3222,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // initialise focal length scale factor estimator states flowStates[0] = 1.0f; @@ -3217,7 +3237,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -3293,7 +3312,6 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); - } void AttPosEKF::GetFilterState(struct ekf_status_report *err) @@ -3310,13 +3328,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); - - // err->velHealth = current_ekf_state.velHealth; - // err->posHealth = current_ekf_state.posHealth; - // err->hgtHealth = current_ekf_state.hgtHealth; - // err->velTimeout = current_ekf_state.velTimeout; - // err->posTimeout = current_ekf_state.posTimeout; - // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index c5517e38b6..9b23f4df44 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -288,7 +288,6 @@ public: * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -307,12 +306,6 @@ public: static void quat2eul(float (&eul)[3], const float (&quat)[4]); - static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - - static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); - - static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); - //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;} @@ -362,8 +355,6 @@ public: */ void ResetVelocity(); - void ZeroVariables(); - void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); @@ -381,6 +372,12 @@ public: * true if the vehicle moves like a Fixed Wing, false otherwise **/ void setIsFixedWing(const bool fixedWing); + + /** + * @brief + * Reset internal filter states and clear all variables to zero value + */ + void ZeroVariables(); protected: @@ -409,7 +406,7 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); void ResetStoredStates(); - + private: bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 470eb4e2c2..284a099023 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -114,6 +114,27 @@ Mat3f Mat3f::transpose() const return ret; } +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNEDr[0] = gpsGndSpd*cosf(gpsCourse); + velNEDr[1] = gpsGndSpd*sinf(gpsCourse); + velNEDr[2] = gpsVelD; +} + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) +{ + posNEDr[0] = earthRadius * (lat - latReference); + posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNEDr[2] = -(hgt - hgtReference); +} + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) +{ + lat = latRef + (double)posNEDi[0] * earthRadiusInv; + lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNEDi[2]; +} + // overload + operator to provide a vector addition Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2) { @@ -140,7 +161,7 @@ Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn) Vector3f vecOut; vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; return vecOut; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index c137209ffe..788fb85ec9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -45,7 +45,6 @@ #define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f -#define pi 3.141592657f #define earthRate 0.000072921f #define earthRadius 6378145.0 #define earthRadiusInv 1.5678540e-7 @@ -128,3 +127,9 @@ struct ekf_status_report { }; void ekf_debug(const char *fmt, ...); + +void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference); + +void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 71b387b1e7..bd14863247 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -97,8 +97,9 @@ usage(const char *reason) int fixedwing_backside_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index db30af4160..aa82a4f596 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // rate of climb -// this is what rate of climb is commanded (in m/s) +// this is what rate of climb is commanded (in m/s) // when the pitch stick is fully defelcted in simple mode PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); 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 06df5fd974..0a8d07fed9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1079,7 +1079,8 @@ FixedwingAttitudeControl::task_main() /* Only publish if any of the proper modes are enabled */ if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled) + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { @@ -1133,8 +1134,9 @@ FixedwingAttitudeControl::start() int fw_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: fw_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6ab3ddbfc8..6b248cbe2e 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,7 +136,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); * @max 2.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); /** * Roll rate proportional Gain 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 427df9739d..34265d6a4e 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 @@ -1638,8 +1638,9 @@ FixedwingPositionControl::start() int fw_pos_control_l1_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index ae6867d382..4c2db0c64d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -65,19 +65,19 @@ public: * Control in altitude setpoint and speed mode */ int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, - float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, - float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); /* * Reset all integrators diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 58a1e9f6b1..fff506865a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -157,8 +157,8 @@ PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); /** * Minimal Pitch Setpoint in Degrees * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -167,8 +167,8 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); /** * Maximal Pitch Setpoint in Degrees * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -192,8 +192,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); @@ -202,8 +202,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); * D gain for the altitude control * Maps the change of altitude error to the flight path angle setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); @@ -219,8 +219,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); /** * Minimal flight path angle setpoint * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -229,8 +229,8 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); /** * Maximal flight path angle setpoint * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -254,8 +254,8 @@ PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f); * P gain for the airspeed control * Maps the airspeed error to the acceleration setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); @@ -264,8 +264,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); * D gain for the airspeed control * Maps the change of airspeed error to the acceleration setpoint * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); @@ -296,8 +296,8 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); /** * Minimal throttle during takeoff * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); @@ -305,8 +305,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); /** * Maximal throttle during takeoff * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); @@ -314,8 +314,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); /** * Minimal pitch during takeoff * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -324,8 +324,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); /** * Maximal pitch during takeoff * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -334,8 +334,8 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); /** * Minimal throttle in underspeed mode * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); @@ -343,8 +343,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); /** * Maximal throttle in underspeed mode * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); @@ -352,8 +352,8 @@ PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); /** * Minimal pitch in underspeed mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -362,8 +362,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); /** * Maximal pitch in underspeed mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -372,8 +372,8 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); /** * Minimal throttle in landing mode (only last phase of landing) * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); @@ -381,8 +381,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); /** * Maximal throttle in landing mode (only last phase of landing) * - * @min 0.0f - * @max 1.0f + * @min 0.0 + * @max 1.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); @@ -390,8 +390,8 @@ PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); /** * Minimal pitch in landing mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -400,8 +400,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); /** * Maximal pitch in landing mode * - * @min -90.0f - * @max 90.0f + * @min -90.0 + * @max 90.0 * @unit deg * @group mTECS */ @@ -410,8 +410,8 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); /** * Integrator Limit for Total Energy Rate Control * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); @@ -419,8 +419,8 @@ PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); /** * Integrator Limit for Energy Distribution Rate Control * - * @min 0.0f - * @max 10.0f + * @min 0.0 + * @max 10.0 * @group mTECS */ PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index b759b429ef..2ff3fc2767 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -184,11 +184,13 @@ int gpio_led_main(int argc, char *argv[]) warnx("start, using pin: %s", pin_name); exit(0); } + } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); exit(0); + } else { errx(1, "not running"); } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a4fbb9861f..7c830fc072 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -46,8 +46,8 @@ LandDetector::LandDetector() : _landDetectedPub(-1), _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _taskShouldExit(false), + _taskIsRunning(false) { // ctor } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 70df91f43c..8c57416b56 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -102,13 +102,13 @@ private: int _actuatorsSub; int _armingSub; int _parameterSub; - int _attitudeSub; + int _attitudeSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; - struct vehicle_attitude_s _vehicleAttitude; + struct vehicle_attitude_s _vehicleAttitude; uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 011567e577..b4b7c33a20 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -178,7 +178,7 @@ static int land_detector_start(const char *mode) int land_detector_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { goto exiterr; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9afe74af1c..796d5cbf28 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,14 +64,18 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); /** * Use/Accept HIL GPS message (even if not in HIL mode) + * * If set to 1 incomming HIL GPS messages are parsed. + * * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); /** * Forward external setpoint messages + * * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard * control mode + * * @group MAVLink */ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f8e819ce7a..ba049bac40 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -746,7 +746,7 @@ Mavlink::get_free_tx_buf() } void -Mavlink::send_message(const uint8_t msgid, const void *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ @@ -780,7 +780,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* use mavlink's internal counter for the TX seq */ buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; + buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; buf[5] = msgid; /* payload */ @@ -1361,7 +1361,7 @@ Mavlink::task_main(int argc, char *argv[]) /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); - _parameters_manager->set_interval(interval_from_rate(30.0f)); + _parameters_manager->set_interval(interval_from_rate(120.0f)); LL_APPEND(_streams, _parameters_manager); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index baaa7bc139..c285bc4052 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -143,6 +143,13 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static int start_helper(int argc, char *argv[]); /** @@ -162,12 +169,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const uint8_t msgid, const void *msg); + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); /** * Resend message as is, don't change sequence number and CRC. */ - void resend_message(mavlink_message_t *msg); + void resend_message(mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -309,7 +316,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -331,9 +338,9 @@ private: unsigned _bytes_txerr; unsigned _bytes_rx; uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; + float _rate_tx; + float _rate_txerr; + float _rate_rx; struct telemetry_status_s _rstatus; ///< receive status @@ -363,16 +370,9 @@ private: void mavlink_update_system(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static unsigned int interval_from_rate(float rate); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d6b60e227..5adfea36b6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1867,29 +1867,8 @@ protected: struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { - const unsigned port_width = 8; - /* deprecated message (but still needed for compatibility!) */ - for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { - /* channels are sent in MAVLink main loop at a fixed interval */ - mavlink_rc_channels_raw_t msg; - - msg.time_boot_ms = rc.timestamp_publication / 1000; - msg.port = i; - msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX; - msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; - msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; - msg.rssi = rc.rssi; - - _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); - } - - /* new message */ + /* send RC channel data and RSSI */ mavlink_rc_channels_t msg; msg.time_boot_ms = rc.timestamp_publication / 1000; @@ -1913,55 +1892,7 @@ protected: msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - /* RSSI has a max value of 100, and when Spektrum or S.BUS are - * available, the RSSI field is invalid, as they do not provide - * an RSSI measurement. Use an out of band magic value to signal - * these digital ports. XXX revise MAVLink spec to address this. - * One option would be to use the top bit to toggle between RSSI - * and input source mode. - * - * Full RSSI field: 0b 1 111 1111 - * - * ^ If bit is set, RSSI encodes type + RSSI - * - * ^ These three bits encode a total of 8 - * digital RC input types. - * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 - * ^ These four bits encode a total of - * 16 RSSI levels. 15 = full, 0 = no signal - * - */ - - /* Initialize RSSI with the special mode level flag */ - msg.rssi = (1 << 7); - - /* Set RSSI */ - msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; - - switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: - /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; - case RC_INPUT_SOURCE_UNKNOWN: - // do nothing - break; - } - - if (rc.rc_lost) { - /* RSSI is by definition zero */ - msg.rssi = 0; - } + msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index e9858b73c6..20d7cfdbbe 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -74,7 +74,6 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; - _mavlink->send_statustext_info("[pm] sending list"); } break; } @@ -94,7 +93,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ - param_t param = param_find(name); + param_t param = param_find_no_notification(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; @@ -127,7 +126,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ - send_param(param_find(name)); + send_param(param_find_no_notification(name)); } else { /* when index is >= 0, send this parameter again */ @@ -184,8 +183,23 @@ MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { - send_param(param_for_index(_send_all_index)); - _send_all_index++; + + /* skip if no space is available */ + if (_mavlink->get_free_tx_buf() < get_size()) { + return; + } + + /* look for the first parameter which is used */ + param_t p; + do { + p = param_for_index(_send_all_index); + _send_all_index++; + } while (p != PARAM_INVALID && !param_used(p)); + + if (p != PARAM_INVALID) { + send_param(p); + } + if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } @@ -209,8 +223,8 @@ MavlinkParametersManager::send_param(param_t param) return; } - msg.param_count = param_count(); - msg.param_index = param_get_index(param); + msg.param_count = param_count_used(); + msg.param_index = param_get_used_index(param); /* copy parameter name */ strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e3f274b8bf..faede15cb9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _land_detector_pub(-1), + _time_offset_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -157,6 +158,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_optical_flow_rad(msg); break; + case MAVLINK_MSG_ID_PING: + handle_message_ping(msg); + break; + case MAVLINK_MSG_ID_SET_MODE: handle_message_set_mode(msg); break; @@ -725,8 +730,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time - vision_position.timestamp_computer = pos.usec; + vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time + vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time vision_position.x = pos.x; vision_position.y = pos.y; vision_position.z = pos.z; @@ -946,6 +951,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode( msg, &ping); + if ((mavlink_system.sysid == ping.target_system) && + (mavlink_system.compid == ping.target_component)) { + mavlink_message_t msg_out; + mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); + } +} + void MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) { @@ -996,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); + struct time_offset_s tsync_offset; + memset(&tsync_offset, 0, sizeof(tsync_offset)); + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -1022,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } } + tsync_offset.offset_ns = _time_offset ; + + if (_time_offset_pub < 0) { + _time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset); + + } else { + orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset); + } + } void @@ -1505,9 +1535,12 @@ void MavlinkReceiver::print_status() } -uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) { - return usec - (_time_offset / 1000) ; + if(_time_offset > 0) + return usec - (_time_offset / 1000) ; + else + return hrt_absolute_time(); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b510dbbb77..2b6174f8fe 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -73,6 +73,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -127,6 +128,7 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_system_time(mavlink_message_t *msg); void handle_message_timesync(mavlink_message_t *msg); @@ -137,9 +139,10 @@ private: void *receive_thread(void *arg); /** - * Convert remote nsec timestamp to local hrt time (usec) + * Convert remote timestamp to local hrt time (usec) + * Use timesync if available, monotonic boot time otherwise */ - uint64_t to_hrt(uint64_t nsec); + uint64_t sync_stamp(uint64_t usec); /** * Exponential moving average filter to smooth time offset */ @@ -176,6 +179,7 @@ private: orb_advert_t _rc_pub; orb_advert_t _manual_pub; orb_advert_t _land_detector_pub; + orb_advert_t _time_offset_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; @@ -188,6 +192,6 @@ private: uint64_t _time_offset; /* do not allow copying this class */ - MavlinkReceiver(const MavlinkReceiver&); - MavlinkReceiver operator=(const MavlinkReceiver&); + MavlinkReceiver(const MavlinkReceiver &); + MavlinkReceiver operator=(const MavlinkReceiver &); }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5b9e45d3ec..57a92c8b58 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -80,6 +80,7 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); + if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index ef22dc443a..5e39bbbdf9 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -89,7 +89,7 @@ public: virtual unsigned get_size() = 0; protected: - Mavlink * _mavlink; + Mavlink *_mavlink; unsigned int _interval; virtual void send(const hrt_abstime t) = 0; @@ -98,8 +98,8 @@ private: hrt_abstime _last_sent; /* do not allow top copying this class */ - MavlinkStream(const MavlinkStream&); - MavlinkStream& operator=(const MavlinkStream&); + MavlinkStream(const MavlinkStream &); + MavlinkStream &operator=(const MavlinkStream &); }; 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 401cf05f17..3a0dfe4c3b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -77,6 +77,8 @@ #include #include #include +#include +#include #include #include #include @@ -129,10 +131,12 @@ private: int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ + int _motor_limits_sub; /**< motor limits subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _controller_status_pub; /**< controller status publication */ 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 */ @@ -147,6 +151,8 @@ private: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct multirotor_motor_limits_s _motor_limits; /**< motor limits */ + struct mc_att_ctrl_status_s _controller_status; /**< controller status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; @@ -166,15 +172,20 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; + param_t roll_rate_max; + param_t pitch_rate_max; param_t yaw_rate_max; param_t man_roll_max; @@ -191,8 +202,13 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ - float yaw_rate_max; /**< max yaw rate */ + + float roll_rate_max; + float pitch_rate_max; + float yaw_rate_max; + math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ float man_roll_max; float man_pitch_max; @@ -251,6 +267,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for vehicle motor limits status. + */ + void vehicle_motor_limits_poll(); + /** * Shim for calling task_main from task_create. */ @@ -292,6 +313,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _controller_status_pub(-1), _rates_sp_id(0), _actuators_id(0), @@ -310,17 +332,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + memset(&_motor_limits, 0, sizeof(_motor_limits)); + memset(&_controller_status,0,sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; + _params.roll_rate_max = 0.0f; + _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; + _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _rates_prev.zero(); @@ -335,15 +363,20 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); + _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); @@ -394,6 +427,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +439,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,12 +451,20 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - /* manual control scale */ + /* angular rate limits */ + param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); + _params.mc_rate_max(0) = math::radians(_params.roll_rate_max); + param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); + _params.mc_rate_max(1) = math::radians(_params.pitch_rate_max); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); + + /* manual attitude control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); @@ -427,7 +472,7 @@ MulticopterAttitudeControl::parameters_update() _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* acro control scale */ + /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); param_get(_params_handles.acro_pitch_max, &v); @@ -539,6 +584,18 @@ MulticopterAttitudeControl::vehicle_status_poll() } } +void +MulticopterAttitudeControl::vehicle_motor_limits_poll() +{ + /* check if there is a new message */ + bool updated; + orb_check(_motor_limits_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -625,8 +682,10 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* limit yaw rate */ - _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); + } /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; @@ -653,11 +712,11 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { + /* update integral only if not saturated on low limit and if motor commands are not saturated */ + if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -692,6 +751,7 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); @@ -744,6 +804,7 @@ MulticopterAttitudeControl::task_main() arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); + vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -804,6 +865,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; + _controller_status.roll_rate_integ = _rates_int(0); + _controller_status.pitch_rate_integ = _rates_int(1); + _controller_status.yaw_rate_integ = _rates_int(2); + _controller_status.timestamp = hrt_absolute_time(); + if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); @@ -814,6 +880,13 @@ MulticopterAttitudeControl::task_main() } } + + /* publish controller status */ + if(_controller_status_pub > 0) { + orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); + } else { + _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); + } } } @@ -849,8 +922,9 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { errx(1, "usage: mc_att_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 302551959b..3f19a51f06 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -82,6 +82,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + /** * Pitch P gain * @@ -123,6 +133,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + /** * Yaw P gain * @@ -164,6 +184,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + /** * Yaw feed forward * @@ -175,6 +205,30 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +/** + * Max roll rate + * + * Limit for roll rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_MAX, 360.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f); + /** * Max yaw rate * diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 52ba369c1a..1242c7fe37 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -88,9 +88,9 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ - px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ - px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + px4::Publisher *_att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher *_v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher *_actuators_0_pub; /**< attitude actuator controls publication */ px4::NodeHandle _n; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h index a1bf44fc96..1d76afb82d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h @@ -82,7 +82,7 @@ public: */ ~MulticopterAttitudeControlSim(); - /* setters and getters for interface with euroc-gazebo simulator */ + /* setters and getters for interface with rotors-gazebo simulator */ void set_attitude(const Eigen::Quaternion attitude); void set_attitude_rates(const Eigen::Vector3d &angular_rate); void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 40eb498b49..dec1e1745d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); int mc_att_control_m_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_att_control_m {start|stop|status}"); } 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 58d20ebef9..5191a4de3b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1308,8 +1308,8 @@ MulticopterPositionControl::task_main() /* save thrust setpoint for logging */ _local_pos_sp.acc_x = thrust_sp(0); - _local_pos_sp.acc_x = thrust_sp(1); - _local_pos_sp.acc_x = thrust_sp(2); + _local_pos_sp.acc_y = thrust_sp(1); + _local_pos_sp.acc_z = thrust_sp(2); _att_sp.timestamp = hrt_absolute_time(); @@ -1440,7 +1440,7 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_pos_control {start|stop|status}"); } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 40268358ad..d135eecfbb 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -922,8 +922,8 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti /* save thrust setpoint for logging */ _local_pos_sp_msg.data().acc_x = thrust_sp(0); - _local_pos_sp_msg.data().acc_x = thrust_sp(1); - _local_pos_sp_msg.data().acc_x = thrust_sp(2); + _local_pos_sp_msg.data().acc_y = thrust_sp(1); + _local_pos_sp_msg.data().acc_z = thrust_sp(2); _att_sp_msg.data().timestamp = get_time_micros(); diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 1082061f64..0db67d83ff 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -54,7 +54,7 @@ extern int main(int argc, char **argv); extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); int mc_pos_control_m_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: mc_pos_control_m {start|stop|status}"); } diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 87a6e023a5..7925809f12 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -186,7 +186,7 @@ DataLinkLoss::advance_dll() if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); @@ -194,19 +194,19 @@ DataLinkLoss::advance_dll() } else { if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } else { /* comms hold wp not active, fly to airfield home directly */ warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } } break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -215,7 +215,7 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; warnx("time is up, state should have been changed manually by now"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 6780c0c88c..9abc012cf2 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -54,7 +54,7 @@ * * @unit seconds * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); @@ -64,8 +64,8 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * Latitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0.0 - * @group DLL + * @min 0 + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); @@ -75,8 +75,8 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); * Longitude of comms hold waypoint * * @unit degrees * 1e7 - * @min 0.0 - * @group DLL + * @min 0 + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); @@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); * * @unit m * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); * * @unit seconds * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); @@ -107,7 +107,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group DLL + * @group Data Link Loss * @min 0 * @max 1000 */ @@ -119,7 +119,7 @@ PARAM_DEFINE_INT32(NAV_DLL_N, 2); * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to * airfield home * - * @group DLL + * @group Data Link Loss * @min 0 * @max 1 */ diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index e007b208b2..b0130a1f5c 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -140,7 +140,7 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); + mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9bc9be245c..1b66107347 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,6 +39,7 @@ */ #include "geofence.h" +#include #include #include #include @@ -49,6 +50,15 @@ #include #include #include +#include +#include + +#define GEOFENCE_OFF 0 +#define GEOFENCE_FILE_ONLY 1 +#define GEOFENCE_MAX_DISTANCES_ONLY 2 +#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 + +#define GEOFENCE_RANGE_WARNING_LIMIT 3000000 /* Oddly, ERROR is not defined for C++ */ @@ -60,13 +70,19 @@ static const int ERROR = -1; Geofence::Geofence() : SuperBlock(NULL, "GF"), _fence_pub(-1), + _home_pos{}, + _home_pos_set(false), + _last_horizontal_range_warning(0), + _last_vertical_range_warning(0), _altitude_min(0), _altitude_max(0), _verticesCount(0), - _param_geofence_on(this, "ON"), + _param_geofence_mode(this, "MODE"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), + _param_max_hor_distance(this, "MAX_HOR_DIST"), + _param_max_ver_distance(this, "MAX_VER_DIST"), _outside_counter(0), _mavlinkFd(-1) { @@ -92,10 +108,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set) { updateParams(); + _home_pos = home_pos; + _home_pos_set = home_position_set; + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); @@ -118,13 +138,48 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { + if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { + int32_t max_horizontal_distance = _param_max_hor_distance.get(); + int32_t max_vertical_distance = _param_max_ver_distance.get(); + + if (max_horizontal_distance > 0 || max_vertical_distance > 0) { + if (_home_pos_set) { + float dist_xy = -1.0f; + float dist_z = -1.0f; + get_distance_to_point_global_wgs84(lat, lon, altitude, + _home_pos.lat, _home_pos.lon, _home_pos.alt, + &dist_xy, &dist_z); + + if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { + if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", + (double)(dist_z - max_vertical_distance)); + _last_vertical_range_warning = hrt_absolute_time(); + } + + return false; + } + + if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { + if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", + (double)(dist_xy - max_horizontal_distance)); + _last_horizontal_range_warning = hrt_absolute_time(); + } + + return false; + } + } + } + } + bool inside_fence = inside_polygon(lat, lon, altitude); if (inside_fence) { _outside_counter = 0; return inside_fence; - } { + } else { _outside_counter++; if (_outside_counter > _param_counter_threshold.get()) { @@ -139,8 +194,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { - /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) { + /* Return true if geofence is disabled or only checking max distances */ + if ((_param_geofence_mode.get() == GEOFENCE_OFF) + || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { return true; } @@ -366,7 +422,7 @@ Geofence::loadFromFile(const char *filename) } else { warnx("Geofence: import error"); - mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); + mavlink_log_critical(_mavlinkFd, "Geofence import error"); } error: diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9d647cb68a..96764cc976 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,8 +45,10 @@ #include #include #include +#include #include #include +#include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" @@ -76,7 +78,9 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, + const struct home_position_s home_pos, bool home_position_set); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -103,16 +107,24 @@ public: private: orb_advert_t _fence_pub; /**< publish fence topic */ + home_position_s _home_pos; + bool _home_pos_set; + + hrt_abstime _last_horizontal_range_warning; + hrt_abstime _last_vertical_range_warning; + float _altitude_min; float _altitude_max; unsigned _verticesCount; /* Params */ - control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_geofence_mode; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; + control::BlockParamInt _param_max_hor_distance; + control::BlockParamInt _param_max_ver_distance; uint8_t _outside_counter; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index fca3918e1d..f3e7d4c84a 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -48,16 +48,15 @@ */ /** - * Enable geofence. + * Geofence mode. * - * Set to 1 to enable geofence. - * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both * * @min 0 - * @max 1 + * @max 3 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ON, 1); +PARAM_DEFINE_INT32(GF_MODE, 0); /** * Geofence altitude mode @@ -94,3 +93,21 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_COUNT, -1); + +/** + * Max horizontal distance in meters. + * + * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); + +/** + * Max vertical distance in meters. + * + * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index e370796c07..0ca69f0f75 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -161,12 +161,12 @@ GpsFailure::advance_gpsf() case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; warnx("gpsf loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; warnx("gpsf terminate"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination"); warnx("mavlink sent"); break; case GPSF_STATE_TERMINATE: diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index 39d179eed7..98104af29e 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -55,7 +55,7 @@ * * @unit seconds * @min 0.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); * @unit deg * @min 0.0 * @max 30.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); * @unit deg * @min -30.0 * @max 30.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); * * @min 0.0 * @max 1.0 - * @group GPSF + * @group GPS Failure Navigation */ PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a94082d6f0..c1763ba93d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -449,7 +449,7 @@ Mission::set_mission_items() } /* check if we already above takeoff altitude */ - if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + if (_navigator->get_global_position()->alt < takeoff_alt) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 52ccebac9f..dce7d45171 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -159,7 +159,7 @@ MissionBlock::is_mission_item_reached() _time_first_inside_orbit = now; // if (_mission_item.time_inside > 0.01f) { - // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs", // (double)_mission_item.time_inside); // } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index adf50bc391..ec3e305825 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -83,9 +83,9 @@ protected: */ void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - /** - * Set previous position setpoint to current setpoint - */ + /** + * Set previous position setpoint to current setpoint + */ void set_previous_pos_setpoint(); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 389cdd0d2d..904f442389 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -121,7 +121,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss } if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { - mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + mavlink_log_critical(_mavlink_fd, "Geofence violation waypoint %d", i); return false; } } @@ -203,25 +203,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } else { /* Landing waypoint is above altitude of slope at the given waypoint distance */ - mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); - mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close"); + mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", (double)(slope_alt_req), (double)(wp_distance_req - wp_distance)); return false; } } else { /* Landing waypoint is above last waypoint */ - mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint"); return false; } } else { /* Last wp is in flare region */ //xxx give recommendations - mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region"); return false; } } else { - mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint"); return false; } } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 96c9209d3d..9a77a6dc25 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -78,7 +78,8 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt); }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d9d911d9c7..d2acce7899 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -186,6 +186,8 @@ 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 */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ad2349c947..9691e26a8d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,6 +123,7 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, + _home_position_set(false), _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -203,7 +204,13 @@ Navigator::sensor_combined_update() void Navigator::home_position_update() { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + bool updated = false; + orb_check(_home_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + _home_position_set = true; + } } void @@ -392,7 +399,7 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { @@ -402,7 +409,7 @@ Navigator::task_main() /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { - mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); + mavlink_log_critical(_mavlink_fd, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 2f322031c3..f7ccda0cbd 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -58,7 +58,8 @@ NavigatorMode::~NavigatorMode() } void -NavigatorMode::run(bool active) { +NavigatorMode::run(bool active) +{ if (active) { if (_first_run) { /* first run */ diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index de5545dcb1..0839d9be60 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -92,8 +92,8 @@ private: /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ - NavigatorMode(const NavigatorMode&); - NavigatorMode operator=(const NavigatorMode&); + NavigatorMode(const NavigatorMode &); + NavigatorMode operator=(const NavigatorMode &); }; #endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 5f8f8d02f1..ef4a8dc0c7 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -94,8 +94,8 @@ PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); * Latitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0.0 - * @group DLL + * @min 0 + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); @@ -105,8 +105,8 @@ PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); * Longitude of airfield home waypoint * * @unit degrees * 1e7 - * @min 0.0 - * @group DLL + * @min 0 + * @group Data Link Loss */ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); @@ -117,6 +117,6 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); * * @unit m * @min 0.0 - * @group DLL + * @group Data Link Loss */ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index a7cde6325d..40bf7f0223 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -155,11 +155,11 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: if (_param_loitertime.get() > 0.0f) { warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; } else { warnx("RC loss, OBC mode, slip loiter, terminate"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); @@ -169,7 +169,7 @@ RCLoss::advance_rcl() case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; warnx("time is up, no RC regain, terminating"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->set_mission_result_updated(); reset_mission_item_reached(); diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index f1a01c73b5..f48aabc808 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -55,6 +55,6 @@ * * @unit seconds * @min -1.0 - * @group RCL + * @group Radio Signal Loss */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index b6c4b8fdff..c1ed8cb7c1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -89,7 +89,7 @@ RTL::on_activation() /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt @@ -146,7 +146,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home", (int)(climb_alt - _navigator->get_home_position()->alt)); break; } @@ -177,7 +177,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } @@ -197,7 +197,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = false; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home", (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); break; } @@ -222,10 +222,10 @@ RTL::set_rtl_item() _navigator->set_can_loiter_at_sp(true); if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside); } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter"); } break; } @@ -245,7 +245,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); break; } @@ -264,7 +264,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); break; } diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 10394fed18..7800a6f03f 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -55,7 +55,7 @@ * @unit meters * @min 20 * @max 200 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * @unit meters * @min 0 * @max 150 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); @@ -81,7 +81,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); * @unit meters * @min 2 * @max 100 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); @@ -94,6 +94,6 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30); * @unit seconds * @min -1 * @max 300 - * @group RTL + * @group Return To Land */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index a36a4688d0..c70cbeb0ea 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,6 +15,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) if (!isfinite(acc)) { acc = 0.0f; } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; x[1] += acc * dt; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ec297e7f0c..437395b1f1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -134,7 +134,7 @@ static void usage(const char *reason) */ int position_estimator_inav_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { usage("missing command"); } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index ed98e222ec..aedb478faa 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -282,8 +282,8 @@ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); * * Set to the appropriate key (328754) to disable vision input. * - * @min 0.0 - * @max 1.0 + * @min 0 + * @max 1 * @group Position Estimator INAV */ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); @@ -295,9 +295,8 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); * the system uses the combined attitude / position * filter framework. * - * @min 0.0 - * @max 1.0 - * @unit s + * @min 0 + * @max 1 * @group Position Estimator INAV */ PARAM_DEFINE_INT32(INAV_ENABLED, 1); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e04ffc9400..ac004f2127 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -99,6 +99,9 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (*st24_updated) { + /* ensure ADC RSSI is disabled */ + r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI); + *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -116,14 +119,14 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = RC_INPUT_RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*sumd_updated) { - *rssi = sumd_rssi; + /* not setting RSSI since SUMD does not provide one */ r_raw_rc_count = sumd_channel_count; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; @@ -187,12 +190,20 @@ controls_tick() { /* use 1:1 scaling on 3.3V ADC input */ unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 and lowpass */ + rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f); + if (rssi > RC_INPUT_RSSI_MAX) { + rssi = RC_INPUT_RSSI_MAX; + } } } #endif + /* zero RSSI if signal is lost */ + if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) { + rssi = 0; + } + perf_begin(c_gather_dsm); bool dsm_updated, st24_updated, sumd_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -215,22 +226,26 @@ controls_tick() { if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - rssi = 255; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - rssi = 100; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } if (sbus_failsafe) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - rssi = 0; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + rssi = sbus_rssi; + } + } perf_end(c_gather_sbus); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 66f0969de1..6fa26d4fff 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -230,7 +230,7 @@ mixer_tick(void) /* poor mans mutex */ in_mixer = true; - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); in_mixer = false; /* the pwm limit call takes care of out of band errors */ @@ -272,8 +272,9 @@ mixer_tick(void) if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); + } /* set S.BUS1 or S.BUS2 outputs */ @@ -285,8 +286,9 @@ mixer_tick(void) } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); + } /* set S.BUS1 or S.BUS2 outputs */ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) @@ -451,7 +453,7 @@ mixer_set_failsafe() unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 874dc0c398..cbafa36410 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -133,6 +133,11 @@ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ +#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT (1 << 0) /**< at least one actuator output has reached lower limit */ +#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT (1 << 1) /**< at least one actuator output has reached upper limit */ +#define PX4IO_P_STATUS_MIXER_YAW_LIMIT (1 << 2) /**< yaw control is limited because it causes output clipping */ + /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 93a33490fa..8ddf45a12d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -51,7 +51,8 @@ /* * Constants and limits. */ -#define PX4IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 16 +#define PX4IO_SERVO_HARDWARE_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 @@ -97,8 +98,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] -#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) +#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER] #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] @@ -219,7 +221,8 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_channels); extern void sbus1_output(uint16_t *values, uint16_t num_values); extern void sbus2_output(uint16_t *values, uint16_t num_values); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f0c2cfd26d..e7976446cd 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,6 +90,7 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_MIXER] = 0, }; /** @@ -284,7 +285,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ if (*values != PWM_IGNORE_THIS_CHANNEL) { @@ -543,8 +544,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 50) { - value = 50; + if (value < 25) { + value = 25; } if (value > 400) { value = 400; @@ -553,8 +554,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_ALTRATE: - if (value < 50) { - value = 50; + if (value < 25) { + value = 25; } if (value > 400) { value = 400; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f05..14d8ccca2e 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -142,7 +142,7 @@ sbus1_output(uint16_t *values, uint16_t num_values) value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); /*protect from out of bounds values and limit to 11 bits*/ - if (value > 0x07ff ) { + if (value > 0x07ff) { value = 0x07ff; } @@ -163,8 +163,8 @@ sbus1_output(uint16_t *values, uint16_t num_values) void sbus2_output(uint16_t *values, uint16_t num_values) { - char b = 'B'; - write(sbus_fd, &b, 1); + // XXX S.BUS2 is not implemented, fall back to S.BUS1 + sbus1_output(values, num_values); } bool diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 91a9611d45..6964acf339 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,5 +46,6 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1300 +EXTRACFLAGS = -Wframe-larger-than=1400 +EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7017a65add..8193cf1814 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -99,6 +99,8 @@ #include #include #include +#include +#include #include #include @@ -340,15 +342,32 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } + if (!thread_running) { + warnx("not started\n"); + return 1; + } + if (!strcmp(argv[1], "status")) { - if (thread_running) { - sdlog2_status(); + sdlog2_status(); + return 0; + } - } else { - warnx("not started\n"); - } + if (!strcmp(argv[1], "on")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 1; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; + } - exit(0); + if (!strcmp(argv[1], "off")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 0; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; } sdlog2_usage("unrecognized command"); @@ -627,13 +646,16 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { + if (logging_enabled) { + return; + } + /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); exit(1); } - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -674,6 +696,10 @@ void sdlog2_start_log() void sdlog2_stop_log() { + if (!logging_enabled) { + return; + } + logging_enabled = false; /* wake up write thread one last time */ @@ -739,7 +765,7 @@ int write_version(int fd) }; /* fill version message and write it */ - strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); return write(fd, &log_msg_VER, sizeof(log_msg_VER)); } @@ -1003,6 +1029,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct wind_estimate_s wind_estimate; struct encoders_s encoders; struct vtol_vehicle_status_s vtol_status; + struct time_offset_s time_offset; + struct mc_att_ctrl_status_s mc_att_ctrl_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1047,6 +1075,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; struct log_ENCD_s log_ENCD; + struct log_TSYN_s log_TSYN; + struct log_MACS_s log_MACS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1087,6 +1117,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int tsync_sub; + int mc_att_ctrl_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1118,6 +1150,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.tsync_sub = orb_subscribe(ORB_ID(time_offset)); + subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1216,12 +1250,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (status_updated) { log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; + log_msg.body.log_STAT.main_state = buf_status.main_state; + log_msg.body.log_STAT.arming_state = buf_status.arming_state; + log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; + log_msg.body.log_STAT.load = buf_status.load; LOGBUFFER_WRITE_AND_COUNT(STAT); } @@ -1346,6 +1381,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1401,6 +1439,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1430,6 +1471,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1461,6 +1505,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + log_msg.body.log_ATSP.q_w = buf.att_sp.q_d[0]; + log_msg.body.log_ATSP.q_x = buf.att_sp.q_d[1]; + log_msg.body.log_ATSP.q_y = buf.att_sp.q_d[2]; + log_msg.body.log_ATSP.q_z = buf.att_sp.q_d[3]; LOGBUFFER_WRITE_AND_COUNT(ATSP); } @@ -1783,6 +1831,22 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ENCD); } + /* --- TIMESYNC OFFSET --- */ + if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) { + log_msg.msg_type = LOG_TSYN_MSG; + log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns; + LOGBUFFER_WRITE_AND_COUNT(TSYN); + } + + /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ + if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) { + log_msg.msg_type = LOG_MACS_MSG; + log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ; + log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ; + log_msg.body.log_MACS.yaw_rate_integ = buf.mc_att_ctrl_status.yaw_rate_integ; + LOGBUFFER_WRITE_AND_COUNT(MACS); + } + /* 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 */ @@ -1809,13 +1873,18 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { - float kibibytes = log_bytes_written / 1024.0f; - float mebibytes = kibibytes / 1024.0f; - 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); warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + if (!logging_enabled) { + warnx("standing by"); + } else { + + float kibibytes = log_bytes_written / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + 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); + } } /** @@ -1903,13 +1972,15 @@ void handle_command(struct vehicle_command_s *cmd) switch (cmd->command) { case VEHICLE_CMD_PREFLIGHT_STORAGE: - param = (int)(cmd->param3); + param = (int)(cmd->param3 + 0.5f); if (param == 1) { sdlog2_start_log(); } else if (param == 0) { sdlog2_stop_log(); + } else { + warnx("unknown storage cmd"); } break; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87b4795bca..c6f967af43 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,10 @@ struct log_ATSP_s { float pitch_sp; float yaw_sp; float thrust_sp; + float q_w; + float q_x; + float q_y; + float q_z; }; /* --- IMU - IMU SENSORS --- */ @@ -89,6 +93,9 @@ struct log_IMU_s { float mag_x; float mag_y; float mag_z; + float temp_acc; + float temp_gyro; + float temp_mag; }; /* --- SENS - OTHER SENSORS --- */ @@ -171,10 +178,11 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; - uint8_t failsafe_state; + uint8_t failsafe; float battery_remaining; uint8_t battery_warning; uint8_t landed; + float load; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -441,6 +449,20 @@ struct log_VTOL_s { float airspeed_tot; }; +/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ +#define LOG_TSYN_MSG 43 +struct log_TSYN_s { + uint64_t time_offset; +}; + +/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ +#define LOG_MACS_MSG 42 +struct log_MACS_s { + float roll_rate_integ; + float pitch_rate_integ; + float yaw_rate_integ; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -469,10 +491,10 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT(ATSP, "ffffffff", "RollSP,PitchSP,YawSP,ThrustSP,qw,qx,qy,qz"), + LOG_FORMAT_S(IMU, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU1, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU2, IMU, "ffffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), @@ -480,7 +502,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + 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(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), @@ -509,6 +531,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(TSYN, "Q", "TimeOffset"), + LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index ee492f85a2..b36d56d6d7 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -92,8 +92,9 @@ usage(const char *reason) int segway_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 2) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 272e4b14f3..1bebea2060 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -682,7 +682,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * * @group Sensor Calibration */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation X (Roll) offset @@ -960,6 +960,7 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); /** * Scaling factor for battery voltage sensor on FMU v2. * + * @board CONFIG_ARCH_BOARD_PX4FMU_V2 * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -969,6 +970,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); * * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 * + * @board CONFIG_ARCH_BOARD_AEROCORE * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); @@ -1099,7 +1101,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); @@ -1108,7 +1110,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); @@ -1117,7 +1119,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); @@ -1126,7 +1128,7 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); @@ -1135,7 +1137,7 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); @@ -1144,7 +1146,7 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); @@ -1153,7 +1155,7 @@ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); * * @min 0 * @max 18 - * @group Radio Calibration + * @group Radio Switches */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); @@ -1238,9 +1240,6 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); /** * Threshold for selecting assist mode * - * min:-1 - * max:+1 - * * 0-1 indicate where in the full channel range the threshold sits * 0 : min * 1 : max @@ -1248,15 +1247,17 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); * positive : true when channel>th * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channelth * negative : true when channel 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); + +/** + * Min input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 527ca22100..3fc8627c15 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -145,7 +145,7 @@ #endif static const int ERROR = -1; -#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" /** * Sensor app start / stop handling function @@ -1015,6 +1015,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; raw.accelerometer_errcount = accel_report.error_count; + raw.accelerometer_temp = accel_report.temperature; } orb_check(_accel1_sub, &accel_updated); @@ -1037,6 +1038,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer1_timestamp = accel_report.timestamp; raw.accelerometer1_errcount = accel_report.error_count; + raw.accelerometer1_temp = accel_report.temperature; } orb_check(_accel2_sub, &accel_updated); @@ -1059,6 +1061,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer2_timestamp = accel_report.timestamp; raw.accelerometer2_errcount = accel_report.error_count; + raw.accelerometer2_temp = accel_report.temperature; } } @@ -1086,6 +1089,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; } orb_check(_gyro1_sub, &gyro_updated); @@ -1108,6 +1112,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro1_timestamp = gyro_report.timestamp; raw.gyro1_errcount = gyro_report.error_count; + raw.gyro1_temp = gyro_report.temperature; } orb_check(_gyro2_sub, &gyro_updated); @@ -1130,6 +1135,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro2_timestamp = gyro_report.timestamp; raw.gyro2_errcount = gyro_report.error_count; + raw.gyro2_temp = gyro_report.temperature; } } @@ -1158,6 +1164,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; raw.magnetometer_errcount = mag_report.error_count; + raw.magnetometer_temp = mag_report.temperature; } orb_check(_mag1_sub, &mag_updated); @@ -1181,6 +1188,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer1_timestamp = mag_report.timestamp; raw.magnetometer1_errcount = mag_report.error_count; + raw.magnetometer1_temp = mag_report.temperature; } orb_check(_mag2_sub, &mag_updated); @@ -1204,6 +1212,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer2_timestamp = mag_report.timestamp; raw.magnetometer2_errcount = mag_report.error_count; + raw.magnetometer2_temp = mag_report.temperature; } } @@ -1373,14 +1382,13 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { - gyro_count++; config_ok = true; } } @@ -1388,11 +1396,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR GYRO #%u", s); + if (config_ok) { + gyro_count++; } + + close(fd); } /* run through all accel sensors */ @@ -1440,14 +1448,13 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { - accel_count++; config_ok = true; } } @@ -1455,11 +1462,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR ACCEL #%u", s); + if (config_ok) { + accel_count++; } + + close(fd); } /* run through all mag sensors */ @@ -1471,9 +1478,16 @@ Sensors::parameter_update_poll(bool forced) int fd = open(str, 0); if (fd < 0) { + /* the driver is not running, abort */ continue; } + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + bool config_ok = false; /* run through all stored calibrations */ @@ -1557,14 +1571,13 @@ Sensors::parameter_update_poll(bool forced) } if (failed) { - warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { /* apply new scaling and offsets */ res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { - mag_count++; config_ok = true; } } @@ -1572,11 +1585,11 @@ Sensors::parameter_update_poll(bool forced) } } - close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR MAG #%u", s); + if (config_ok) { + mag_count++; } + + close(fd); } int fd = open(AIRSPEED0_DEVICE_PATH, 0); @@ -1596,7 +1609,7 @@ Sensors::parameter_update_poll(bool forced) close(fd); } - warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); } } @@ -2243,7 +2256,7 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: sensors {start|stop|status}"); } diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ea478a60fa..f5ff0afd4b 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -45,7 +45,7 @@ #include #include -bool circuit_breaker_enabled(const char* breaker, int32_t magic) +bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c97dbc26ff..c76e6c37f3 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e499ae27ac..e5cc034bc9 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -43,7 +43,6 @@ */ #include -#include /** * Circuit breaker for power supply check @@ -56,7 +55,7 @@ * @max 894281 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); /** * Circuit breaker for rate controller output @@ -69,7 +68,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK); * @max 140253 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); /** * Circuit breaker for IO safety @@ -81,7 +80,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL); * @max 22027 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); /** * Circuit breaker for airspeed sensor @@ -93,7 +92,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY); * @max 162128 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); /** * Circuit breaker for flight termination @@ -106,7 +105,7 @@ PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK); * @max 121212 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection @@ -120,4 +119,4 @@ PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM); * @max 284953 * @group Circuit Breaker */ -PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h deleted file mode 100644 index 768bf7f533..0000000000 --- a/src/modules/systemlib/circuit_breaker_params.h +++ /dev/null @@ -1,7 +0,0 @@ -#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0 -#define PARAM_CBRK_RATE_CTRL_DEFAULT 0 -#define PARAM_CBRK_IO_SAFETY_DEFAULT 0 -#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0 -#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212 -#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953 -#define PARAM_CBRK_GPSFAIL_DEFAULT 240024 diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 7aa2f3a5fa..b41896abd9 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -97,7 +97,8 @@ void cpuload_initialize_once() system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; - system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs + system_load.tasks[system_load.total_count].tcb = sched_gettcb( + system_load.total_count); // it is assumed that these static threads have consecutive PIDs system_load.tasks[system_load.total_count].valid = true; } } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 1f3927222a..b674f192d5 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -103,7 +103,7 @@ __EXPORT extern void hx_stream_reset(hx_stream_t stream); /** * Prepare to send a frame. * - * Use this in conjunction with hx_stream_send_next to + * Use this in conjunction with hx_stream_send_next to * set the frame to be transmitted. * * Use hx_stream_send() to write to the stream fd directly. @@ -124,7 +124,7 @@ __EXPORT extern int hx_stream_start(hx_stream_t stream, * calling hx_stream_start first. * * @param stream A handle returned from hx_stream_init. - * @return The byte to send, or -1 if there is + * @return The byte to send, or -1 if there is * nothing left to send. */ __EXPORT extern int hx_stream_send_next(hx_stream_t stream); diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index e951e1e390..f40ac069e1 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -50,7 +50,7 @@ enum MCU_REV { /** * Reports the microcontroller unique id. * - * This ID is guaranteed to be unique for every mcu. + * This ID is guaranteed to be unique for every mcu. * @param uid_96_bit A uint32_t[3] array to copy the data to. */ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); @@ -62,6 +62,6 @@ __EXPORT void mcu_unique_id(uint32_t *uid_96_bit); * @param revstr The full chip name string * @return The silicon revision / version number as integer */ -__EXPORT int mcu_version(char* rev, char** revstr); +__EXPORT int mcu_version(char *rev, char **revstr); __END_DECLS diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 20b1f18ed6..3ab41c5c58 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -151,7 +151,7 @@ NullMixer::NullMixer() : } unsigned -NullMixer::mix(float *outputs, unsigned space) +NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { if (space > 0) { *outputs = 0.0f; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 67ef521b4e..1190683015 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -174,7 +174,7 @@ public: * @param space The number of available entries in the output array; * @return The number of entries in the output array that were populated. */ - virtual unsigned mix(float *outputs, unsigned space) = 0; + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0; /** * Analyses the mix configuration and updates a bitmask of groups @@ -250,7 +250,7 @@ public: MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); ~MixerGroup(); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -346,7 +346,7 @@ public: */ static NullMixer *from_text(const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); }; @@ -411,7 +411,7 @@ public: uint16_t mid, uint16_t max); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); /** @@ -515,7 +515,7 @@ public: const char *buf, unsigned &buflen); - virtual unsigned mix(float *outputs, unsigned space); + virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); virtual void groups_required(uint32_t &groups); private: diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 3ed99fba09..ca5101e657 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -76,8 +76,9 @@ MixerGroup::add_mixer(Mixer *mixer) mpp = &_first; - while (*mpp != nullptr) + while (*mpp != nullptr) { mpp = &((*mpp)->_next); + } *mpp = mixer; mixer->_next = nullptr; @@ -98,13 +99,13 @@ MixerGroup::reset() } unsigned -MixerGroup::mix(float *outputs, unsigned space) +MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg) { Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < space)) { - index += mixer->mix(outputs + index, space - index); + index += mixer->mix(outputs + index, space - index, status_reg); mixer = mixer->_next; } @@ -185,6 +186,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; debug("SUCCESS - buflen: %d", buflen); + } else { /* diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index b354eb518f..e9a8a87ca8 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,10 +36,7 @@ * * Multi-rotor mixers. */ -#include -#include #include - #include #include #include @@ -53,6 +50,8 @@ #include #include +#include + #include "mixer.h" // This file is generated by the multi_tables script which is invoked during the build process @@ -199,7 +198,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } unsigned -MultirotorMixer::mix(float *outputs, unsigned space) +MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); @@ -210,10 +209,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; - _limits.roll_pitch = false; - _limits.yaw = false; - _limits.throttle_upper = false; - _limits.throttle_lower = false; + if (status_reg != NULL) { + (*status_reg) = 0; + } /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -226,7 +224,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; - _limits.yaw = true; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } } /* calculate min and max output values */ @@ -244,14 +244,26 @@ MultirotorMixer::mix(float *outputs, unsigned space) if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); + max_out = 0.0f; + /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; + + /* update max output value */ + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; + } + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } - _limits.roll_pitch = true; } else { - /* roll/pitch mixed without limiting, add yaw control */ + /* roll/pitch mixed without lower side limiting, add yaw control */ for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] += yaw * _rotors[i].yaw_scale; } @@ -261,7 +273,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; - _limits.throttle_upper = true; + + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; + } } else { scale_out = 1.0f; @@ -269,20 +284,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { - if (outputs[i] < _idle_speed) { - _limits.throttle_lower = true; - } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - /* publish/advertise motor limits if running on FMU */ - if (_limits_pub > 0) { - orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); - } else { - _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); - } -#endif return _rotor_count; } diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index c3985b5de6..e48bda6918 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -265,7 +265,7 @@ out: } unsigned -SimpleMixer::mix(float *outputs, unsigned space) +SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { float sum = 0.0f; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index d0d24960ee..8dea6e6cc0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -91,6 +91,9 @@ struct param_wbuf_s { bool unsaved; }; +// XXX this should be param_info_count, but need to work out linking +uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {}; + /** flexible array holding modified parameter values */ UT_array *param_values; @@ -103,6 +106,10 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static void param_set_used_internal(param_t param); + +static param_t param_find_internal(const char *name, bool notification); + /** lock the parameter store */ static void param_lock(void) @@ -205,26 +212,56 @@ param_notify_changes(void) } param_t -param_find(const char *name) +param_find_internal(const char *name, bool notification) { param_t param; /* perform a linear search of the known parameters */ for (param = 0; handle_in_range(param); param++) { - if (!strcmp(param_info_base[param].name, name)) + if (!strcmp(param_info_base[param].name, name)) { + if (notification) { + param_set_used_internal(param); + } return param; + } } /* not found */ return PARAM_INVALID; } +param_t +param_find(const char *name) +{ + return param_find_internal(name, true); +} + +param_t +param_find_no_notification(const char *name) +{ + return param_find_internal(name, false); +} + unsigned param_count(void) { return param_info_count; } +unsigned +param_count_used(void) +{ + unsigned count = 0; + for (unsigned i = 0; i < sizeof(param_changed_storage) / sizeof(param_changed_storage[0]); i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } + } + } + return count; +} + param_t param_for_index(unsigned index) { @@ -243,6 +280,27 @@ param_get_index(param_t param) return -1; } +int +param_get_used_index(param_t param) +{ + if (!handle_in_range(param)) { + return -1; + } + + /* walk all params and count */ + int count = 0; + + for (unsigned i = 0; i < (unsigned)param + 1; i++) { + for (unsigned j = 0; j < 8; j++) { + if (param_changed_storage[i] & (1 << j)) { + count++; + } + } + } + + return count; +} + const char * param_name(param_t param) { @@ -430,6 +488,8 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } out: + param_set_used_internal(param); + param_unlock(); /* @@ -454,6 +514,29 @@ param_set_no_notification(param_t param, const void *val) return param_set_internal(param, val, false, false); } +bool +param_used(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return false; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (1 << bitindex); +} + +void param_set_used_internal(param_t param) +{ + int param_index = param_get_index(param); + if (param_index < 0) { + return; + } + + unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); + param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex); +} + int param_reset(param_t param) { @@ -717,7 +800,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) * Find the parameter this node represents. If we don't know it, * ignore the node. */ - param_t param = param_find(node->name); + param_t param = param_find_no_notification(node->name); if (param == PARAM_INVALID) { debug("ignoring unrecognised parameter '%s'", node->name); @@ -843,15 +926,20 @@ param_load(int fd) } void -param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used) { param_t param; for (param = 0; handle_in_range(param); param++) { /* if requested, skip unchanged values */ - if (only_changed && (param_find_changed(param) == NULL)) + if (only_changed && (param_find_changed(param) == NULL)) { continue; + } + + if (only_used && !param_used(param)) { + continue; + } func(arg, param); } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 69e984a8f4..b29a7e51d6 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -87,9 +87,19 @@ typedef uintptr_t param_t; * * @param name The canonical name of the parameter being looked up. * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + * This call will also set the parameter as "used" in the system, which is used + * to e.g. show the parameter via the RC interface */ __EXPORT param_t param_find(const char *name); +/** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ +__EXPORT param_t param_find_no_notification(const char *name); + /** * Return the total number of parameters. * @@ -97,6 +107,20 @@ __EXPORT param_t param_find(const char *name); */ __EXPORT unsigned param_count(void); +/** + * Return the actually used number of parameters. + * + * @return The number of parameters. + */ +__EXPORT unsigned param_count_used(void); + +/** + * Wether a parameter is in use in the system. + * + * @return True if it has been written or read + */ +__EXPORT bool param_used(param_t param); + /** * Look up a parameter by index. * @@ -113,6 +137,14 @@ __EXPORT param_t param_for_index(unsigned index); */ __EXPORT int param_get_index(param_t param); +/** + * Look up the index of an used parameter. + * + * @param param The parameter to obtain the index for. + * @return The index of the parameter in use, or -1 if the parameter does not exist. + */ +__EXPORT int param_get_used_index(param_t param); + /** * Obtain the name of a parameter. * @@ -254,8 +286,10 @@ __EXPORT int param_load(int fd); * @param arg Argument passed to the function. * @param only_changed If true, the function is only called for parameters whose values have * been changed from the default. + * @param only_changed If true, the function is only called for parameters which have been + * used in one of the running applications. */ -__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed, bool only_used); /** * Set the default parameter file name. diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 6a667ac6fc..6ec9f4b7e2 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -72,7 +72,8 @@ typedef struct { __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, + const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e70b83ccee..035f63bef4 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -39,7 +39,7 @@ #pragma once - __BEGIN_DECLS +__BEGIN_DECLS /** * Check the RC calibration diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 82183b0d72..280c30023d 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -66,10 +66,11 @@ systemreset(bool to_bootloader) /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } + up_systemreset(); /* lock up here */ - while(true); + while (true); } static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); @@ -87,7 +88,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { int pid; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index dbed297744..1a38b981e8 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -250,3 +250,9 @@ ORB_DEFINE(wind_estimate, struct wind_estimate_s); #include "topics/rc_parameter_map.h" ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); + +#include "topics/time_offset.h" +ORB_DEFINE(time_offset, struct time_offset_s); + +#include "topics/mc_att_ctrl_status.h" +ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b45c49788d..73fe0f9361 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,7 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index a61f078ba1..43cee89fe8 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -65,7 +65,7 @@ struct fence_vertex_s { * This is the position of a geofence * */ -struct fence_s { +struct fence_s { unsigned count; /**< number of actual vertices */ struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h index b07e044993..8d32dfc0ae 100644 --- a/src/modules/uORB/topics/geofence_result.h +++ b/src/modules/uORB/topics/geofence_result.h @@ -50,8 +50,7 @@ * @{ */ -struct geofence_result_s -{ +struct geofence_result_s { bool geofence_violated; /**< true if the geofence is violated */ }; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index d747b5f435..02e17cdd7f 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -55,8 +55,7 @@ /** * GPS home position in WGS84 coordinates. */ -struct home_position_s -{ +struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ double lat; /**< Latitude in degrees */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 2ddc529a3f..16e7f2f126 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,8 +53,7 @@ * @{ */ -struct mission_result_s -{ +struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h index 44c51e4d98..589f8a650c 100644 --- a/src/modules/uORB/topics/multirotor_motor_limits.h +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -52,11 +52,10 @@ * Motor limits */ struct multirotor_motor_limits_s { - uint8_t roll_pitch : 1; // roll/pitch limit reached - uint8_t yaw : 1; // yaw limit reached - uint8_t throttle_lower : 1; // lower throttle limit reached - uint8_t throttle_upper : 1; // upper throttle limit reached - uint8_t reserved : 4; + uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit + uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit + uint8_t yaw : 1; // yaw limit reached + uint8_t reserved : 5; // reserved }; /** diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ded82adea2..a19f3dc107 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -83,6 +83,7 @@ struct sensor_combined_s { int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ unsigned gyro_errcount; /**< Error counter for gyro 0 */ + float gyro_temp; /**< Temperature of gyro 0 */ int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ @@ -90,6 +91,7 @@ struct sensor_combined_s { float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer_errcount; /**< Error counter for accel 0 */ + float accelerometer_temp; /**< Temperature of accel 0 */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ @@ -98,36 +100,43 @@ struct sensor_combined_s { float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ unsigned magnetometer_errcount; /**< Error counter for mag 0 */ + float magnetometer_temp; /**< Temperature of mag 0 */ int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ uint64_t gyro1_timestamp; /**< Gyro timestamp */ unsigned gyro1_errcount; /**< Error counter for gyro 1 */ + float gyro1_temp; /**< Temperature of gyro 1 */ int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer1_errcount; /**< Error counter for accel 1 */ + float accelerometer1_temp; /**< Temperature of accel 1 */ int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer1_errcount; /**< Error counter for mag 0 */ + unsigned magnetometer1_errcount; /**< Error counter for mag 1 */ + float magnetometer1_temp; /**< Temperature of mag 1 */ int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ uint64_t gyro2_timestamp; /**< Gyro timestamp */ unsigned gyro2_errcount; /**< Error counter for gyro 1 */ + float gyro2_temp; /**< Temperature of gyro 1 */ int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ unsigned accelerometer2_errcount; /**< Error counter for accel 2 */ + float accelerometer2_temp; /**< Temperature of accel 2 */ int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ unsigned magnetometer2_errcount; /**< Error counter for mag 2 */ + float magnetometer2_temp; /**< Temperature of mag 2 */ float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index ddca19d61e..3c858901c7 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -60,9 +60,9 @@ typedef enum { TECS_MODE_CLIMBOUT } tecs_mode; - /** - * Internal values of the (m)TECS fixed wing speed alnd altitude control system - */ +/** +* Internal values of the (m)TECS fixed wing speed alnd altitude control system +*/ struct tecs_status_s { uint64_t timestamp; /**< timestamp, in microseconds since system start */ diff --git a/src/modules/uORB/topics/time_offset.h b/src/modules/uORB/topics/time_offset.h new file mode 100644 index 0000000000..99e526c765 --- /dev/null +++ b/src/modules/uORB/topics/time_offset.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file time_offset.h + * Time synchronisation offset + */ + +#ifndef TOPIC_TIME_OFFSET_H_ +#define TOPIC_TIME_OFFSET_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Timesync offset for synchronisation with companion computer, GCS, etc. + */ +struct time_offset_s { + + uint64_t offset_ns; /**< time offset between companion system and PX4, in nanoseconds */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(time_offset); + +#endif /* TOPIC_TIME_OFFSET_H_ */ diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index b54f0322b8..44dc6614fe 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -166,7 +166,7 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d * node in /obj if required and publishes the initial data. * * Any number of advertisers may publish to a topic; publications are atomic - * but co-ordination between publishers is not provided by the ORB. + * but co-ordination between publishers is not provided by the ORB. * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. @@ -184,7 +184,8 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ -extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) __EXPORT; /** diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 4f63629a0c..437feb3014 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -58,17 +58,20 @@ SRCS += sensors/sensor_bridge.cpp \ # libuavcan # include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk -SRCS += $(LIBUAVCAN_SRC) +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS + # # libuavcan drivers for STM32 # include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk -SRCS += $(LIBUAVCAN_STM32_SRC) +# Use the relitive path to keep the genrated files in the BUILD_DIR +SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 5348d44180..576e758df5 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,13 +36,15 @@ */ #include "baro.hpp" +#include #include const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), -_sub_air_data(node) +_sub_air_data(node), +_reports(nullptr) { } @@ -53,6 +55,11 @@ int UavcanBarometerBridge::init() return res; } + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + if (_reports == nullptr) + return -1; + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); @@ -61,6 +68,27 @@ int UavcanBarometerBridge::init() return 0; } +ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *baro_buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + while (count--) { + if (_reports->get(baro_buf)) { + ret += sizeof(*baro_buf); + baro_buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; +} + int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -76,6 +104,24 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) case BAROIOCGMSLPRESSURE: { return _msl_pressure; } + case SENSORIOCSPOLLRATE: { + // not supported yet, pretend that everything is ok + return OK; + } + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } default: { return CDev::ioctl(filp, cmd, arg); } @@ -84,11 +130,12 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::baro_report(); + baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + report.error_count = 0; /* * Altitude computation @@ -104,5 +151,8 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + // add to the ring buffer + _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 c7bbc5af8d..6a39eebfb6 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -42,27 +42,31 @@ #include +class RingBuffer; + class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; - UavcanBarometerBridge(uavcan::INode& node); + UavcanBarometerBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + (const uavcan::ReceivedDataStructure &) > AirDataCbBinder; uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; + RingBuffer *_reports; }; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 96ff9404f5..4b94d9306f 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -56,7 +56,7 @@ class UavcanGnssBridge : public IUavcanSensorBridge public: static const char *const NAME; - UavcanGnssBridge(uavcan::INode& node); + UavcanGnssBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } @@ -72,8 +72,8 @@ private: */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > FixCbBinder; uavcan::INode &_node; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index db38aee1d3..b74afeb0be 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -47,7 +47,7 @@ class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase public: static const char *const NAME; - UavcanMagnetometerBridge(uavcan::INode& node); + UavcanMagnetometerBridge(uavcan::INode &node); const char *get_name() const override { return NAME; } @@ -59,9 +59,9 @@ private: void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> + (const uavcan::ReceivedDataStructure &) > MagCbBinder; uavcan::Subscriber _sub_mag; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index b93a95f965..2d5abf9591 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -210,9 +210,9 @@ void UavcanNode::fill_node_info() /* software version */ uavcan::protocol::SoftwareVersion swver; - // Extracting the first 8 hex digits of FW_GIT and converting them to int + // Extracting the first 8 hex digits of GIT_VERSION and converting them to int char fw_git_short[9] = {}; - std::memmove(fw_git_short, FW_GIT, 8); + std::memmove(fw_git_short, GIT_VERSION, 8); assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); @@ -421,7 +421,7 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL); new_output = true; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 19b9b4b484..96b3ec1a60 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -82,7 +82,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& get_node() { return _node; } + 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); @@ -94,7 +94,7 @@ public: void print_info(); - static UavcanNode* instance() { return _instance; } + static UavcanNode *instance() { return _instance; } private: void fill_node_info(); @@ -122,7 +122,7 @@ private: UavcanEscController _esc_controller; - List _sensor_bridges; ///< List of active sensor bridges + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index b7568ce3a4..c45d7888ab 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -57,13 +57,14 @@ void UnitTest::print_results(void) } /// @brief Used internally to the ut_assert macro to print assert failures. -void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line) +void UnitTest::_print_assert(const char *msg, const char *test, const char *file, int line) { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } /// @brief Used internally to the ut_compare macro to print assert failures. -void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +void UnitTest::_print_compare(const char *msg, const char *v1_text, int v1, const char *v2_text, int v2, + const char *file, int line) { warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } 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 74e1efd6cc..2ae10bd274 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -176,7 +176,7 @@ private: bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors float _airspeed_tot; - + float _tilt_control; //*****************Member functions*********************************************************************** void task_main(); //main task @@ -241,6 +241,7 @@ VtolAttitudeControl::VtolAttitudeControl() : flag_idle_mc = true; _airspeed_tot = 0.0f; + _tilt_control = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -521,6 +522,7 @@ void VtolAttitudeControl::fill_mc_att_control_output() //set neutral position for elevons _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon + _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control } /** @@ -763,7 +765,7 @@ void VtolAttitudeControl::task_main() vehicle_battery_poll(); - if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ @@ -877,7 +879,7 @@ VtolAttitudeControl::start() int vtol_att_control_main(int argc, char *argv[]) { - if (argc < 1) { + if (argc < 2) { errx(1, "usage: vtol_att_control {start|stop|status}"); } 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 33752b2c43..6da28b1304 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -43,7 +43,7 @@ /** * VTOL number of engines * - * @min 1.0 + * @min 1 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_MOT_COUNT,0); @@ -92,8 +92,8 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); * If set to one this parameter will cause permanent attitude stabilization in fw mode. * This parameter has been introduced for pure convenience sake. * - * @min 0.0 - * @max 1.0 + * @min 0 + * @max 1 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index bff7aa3133..499a5dd8b1 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -39,7 +39,7 @@ #pragma once #if defined(__PX4_ROS) -typedef const char* PX4TopicHandle; +typedef const char *PX4TopicHandle; #else #include typedef orb_id_t PX4TopicHandle; @@ -68,8 +68,8 @@ public: virtual ~PX4Message() {}; - virtual M& data() {return _data;} - virtual const M& data() const {return _data;} + virtual M &data() {return _data;} + virtual const M &data() const {return _data;} private: M _data; }; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 0c32026f39..abaa6fc60d 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -53,6 +53,7 @@ Commander::Commander() : _msg_parameter_update(), _msg_actuator_armed(), _msg_vehicle_control_mode(), + _msg_vehicle_status(), _msg_offboard_control_mode(), _got_manual_control(false) { @@ -64,10 +65,9 @@ Commander::Commander() : void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { _got_manual_control = true; - px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ - EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + EvalSwitches(msg, _msg_vehicle_status, _msg_vehicle_control_mode); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ @@ -79,7 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; _msg_vehicle_control_mode.flag_armed = false; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_STANDBY; } } else { @@ -87,19 +87,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; _msg_vehicle_control_mode.flag_armed = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); /* Fill parameter update */ if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { @@ -206,12 +206,11 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - px4::vehicle_status msg_vehicle_status; - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; _msg_actuator_armed.armed = true; @@ -223,7 +222,7 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); } } diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index c537c84193..856144389d 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -88,6 +88,7 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::vehicle_status _msg_vehicle_status; px4::offboard_control_mode _msg_offboard_control_mode; bool _got_manual_control; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index fb0b09de1e..328f545c6b 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -49,8 +49,8 @@ DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), - _attitude_sp_pub(_n.advertise("mavros/setpoint/attitude", 1)), - _thrust_sp_pub(_n.advertise("mavros/setpoint/att_throttle", 1)) + _attitude_sp_pub(_n.advertise("mavros/setpoint_attitude/attitude", 1)), + _thrust_sp_pub(_n.advertise("mavros/setpoint_attitude/att_throttle", 1)) { } diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index 7366d7fc61..8a626f255e 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -46,7 +46,7 @@ DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : _n(), - _local_position_sp_pub(_n.advertise("mavros/setpoint/local_position", 1)) + _local_position_sp_pub(_n.advertise("mavros/setpoint_position/local", 1)) { } @@ -66,6 +66,7 @@ int DemoOffboardPositionSetpoints::main() pose.pose.position.z = 1; _local_position_sp_pub.publish(pose); } + return 0; } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffdc..2758979a29 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -57,7 +57,8 @@ Mavlink::Mavlink() : { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); - + _att_sp = {}; + _offboard_control_mode = {}; } int main(int argc, char **argv) @@ -127,10 +128,8 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); - static offboard_control_mode offboard_control_mode; - /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * if attitude or body rate have been used (not ignored) previously and this message only sends @@ -140,45 +139,46 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; } else { - offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode); - - static vehicle_attitude_setpoint att_sp = {}; + _offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(_offboard_control_mode); /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. */ - att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, - &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); - att_sp.R_valid = true; + _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { + mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, + &_att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); + _att_sp.R_valid = true; + } + - if (!offboard_control_mode.ignore_thrust) { - att_sp.thrust = set_attitude_target.thrust; + if (!_offboard_control_mode.ignore_thrust) { + _att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + _att_sp.q_d[i] = set_attitude_target.q[i]; } - att_sp.q_d_valid = true; + _att_sp.q_d_valid = true; } - _v_att_sp_pub.publish(att_sp); + _v_att_sp_pub.publish(_att_sp); //XXX real mavlink publishes rate sp here diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index acb2408f30..8b7d08d242 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -69,6 +69,8 @@ protected: ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; ros::Publisher _force_sp_pub; + vehicle_attitude_setpoint _att_sp; + offboard_control_mode _offboard_control_mode; /** * diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1098ec73b0..002a112b69 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -33,14 +33,14 @@ /** * @file mc_mixer.cpp - * Dummy multicopter mixer for euroc simulator (gazebo) + * Dummy multicopter mixer for rotors simulator (gazebo) * * @author Roman Bapst */ #include #include #include -#include +#include #include class MultirotorMixer @@ -96,7 +96,7 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; -const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { +const MultirotorMixer::Rotor _config_quad_plus_rotorssim[] = { { 0.000000, 1.000000, 1.00 }, { -0.000000, -1.000000, 1.00 }, { 1.000000, 0.000000, -1.00 }, @@ -118,7 +118,7 @@ const MultirotorMixer::Rotor _config_quad_iris[] = { const MultirotorMixer::Rotor *_config_index[5] = { &_config_x[0], &_config_quad_plus[0], - &_config_quad_plus_euroc[0], + &_config_quad_plus_rotorssim[0], &_config_quad_wide[0], &_config_quad_iris[0] }; @@ -129,7 +129,7 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); - _pub = _n.advertise("/mixed_motor_commands", 10); + _pub = _n.advertise("command/motor_speed", 10); if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); @@ -237,7 +237,7 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m mix(); // publish message - mav_msgs::MotorSpeed rotor_vel_msg; + mav_msgs::CommandMotorSpeed rotor_vel_msg; double scaling; double offset; _n.getParamCached("motor_scaling_radps", scaling); diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index ed3a4efa5c..e4273687e7 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -64,14 +64,16 @@ void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP /* Fill px4 position topic with contents from modelstates topic */ int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when //gazebo rearranges indexes. - for(std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + for (std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { if (*it == "iris" || *it == "ardrone") { index = it - msg->name.begin(); break; } } + msg_v_l_pos.xy_valid = true; msg_v_l_pos.z_valid = true; msg_v_l_pos.v_xy_valid = true; diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c deleted file mode 100644 index 3ff5a066c1..0000000000 --- a/src/systemcmds/boardinfo/boardinfo.c +++ /dev/null @@ -1,409 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file boardinfo.c - * autopilot and carrier board information app - */ - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -__EXPORT int boardinfo_main(int argc, char *argv[]); - -struct eeprom_info_s -{ - unsigned bus; - unsigned address; - unsigned page_size; - unsigned page_count; - unsigned page_write_delay; -}; - -/* XXX currently code below only supports 8-bit addressing */ -const struct eeprom_info_s eeprom_info[] = { - {3, 0x57, 8, 16, 3300}, -}; - -struct board_parameter_s { - const char *name; - bson_type_t type; -}; - -const struct board_parameter_s board_parameters[] = { - {"name", BSON_STRING}, /* ascii board name */ - {"vers", BSON_INT32}, /* board version (major << 8) | minor */ - {"date", BSON_INT32}, /* manufacture date */ - {"build", BSON_INT32} /* build code (fab << 8) | tester */ -}; - -const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); - -static int -eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - uint8_t pagebuf[eeprom->page_size + 1]; - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain writes to the page size */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - pagebuf[0] = address & 0xff; - memcpy(pagebuf + 1, buf + address, count); - - struct i2c_msg_s msgv[1] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = pagebuf, - .length = count + 1 - } - }; - - result = I2C_TRANSFER(dev, msgv, 1); - if (result != OK) { - warnx("EEPROM write failed: %d", result); - goto out; - } - usleep(eeprom->page_write_delay); - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static int -eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain transfers to the page size (bus anti-hog) */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - uint8_t addr = address; - struct i2c_msg_s msgv[2] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = &addr, - .length = 1 - }, - { - .addr = eeprom->address, - .flags = I2C_M_READ, - .buffer = buf + address, - .length = count - } - }; - - result = I2C_TRANSFER(dev, msgv, 2); - if (result != OK) { - warnx("EEPROM read failed: %d", result); - goto out; - } - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static void * -idrom_read(const struct eeprom_info_s *eeprom) -{ - uint32_t size = 0xffffffff; - int result; - void *buf = NULL; - - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - goto fail; - } - if (size > (eeprom->page_size * eeprom->page_count)) { - warnx("ID ROM not programmed"); - goto fail; - } - - buf = malloc(size); - if (buf == NULL) { - warnx("could not allocate %d bytes for ID ROM", size); - goto fail; - } - result = eeprom_read(eeprom, buf, size); - if (result != 0) { - warnx("failed reading ID ROM"); - goto fail; - } - return buf; - -fail: - if (buf != NULL) - free(buf); - return NULL; -} - -static void -boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) -{ - struct bson_encoder_s encoder; - int result = 1; - char *state, *token; - unsigned i; - - /* create the encoder and make a writable copy of the spec */ - bson_encoder_init_buf(&encoder, NULL, 0); - - for (i = 0, token = strtok_r(spec, ",", &state); - token && (i < num_parameters); - i++, token = strtok_r(NULL, ",", &state)) { - - switch (board_parameters[i].type) { - case BSON_STRING: - result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); - break; - case BSON_INT32: - result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); - break; - default: - result = 1; - } - if (result) { - warnx("bson append failed for %s<%s>", board_parameters[i].name, token); - goto out; - } - } - bson_encoder_fini(&encoder); - if (i != num_parameters) { - warnx("incorrect parameter list, expected: \",,\""); - result = 1; - goto out; - } - if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { - warnx("data too large for EEPROM"); - result = 1; - goto out; - } - if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { - warnx("buffer length mismatch"); - result = 1; - goto out; - } - warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - - result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - if (result < 0) { - warnc(-result, "error writing EEPROM"); - result = 1; - } else { - result = 0; - } - -out: - free(bson_encoder_buf_data(&encoder)); - - exit(result); -} - -static int -boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) -{ - switch (node->type) { - case BSON_INT32: - printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); - break; - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - printf("%s: %s\n", node->name, buf); - break; - } - case BSON_EOO: - break; - default: - warnx("unexpected node type %d", node->type); - break; - } - return 1; -} - -static void -boardinfo_show(const struct eeprom_info_s *eeprom) -{ - struct bson_decoder_s decoder; - void *buf; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { - while (bson_decoder_next(&decoder) > 0) - ; - } else { - warnx("failed to init decoder"); - } - free(buf); - exit(0); -} - -struct { - const char *property; - const char *value; -} test_args; - -static int -boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - /* reject nodes with non-matching names */ - if (strcmp(node->name, test_args.property)) - return 1; - - /* compare node values to check for a match */ - switch (node->type) { - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - - /* check for a match */ - if (!strcmp(test_args.value, buf)) { - return 2; - } - break; - } - - case BSON_INT32: { - int32_t val = strtol(test_args.value, NULL, 0); - - /* check for a match */ - if (node->i == val) { - return 2; - } - break; - } - - default: - break; - } - - return 1; -} - -static void -boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) -{ - struct bson_decoder_s decoder; - void *buf; - int result = -1; - - if ((property == NULL) || (strlen(property) == 0) || - (value == NULL) || (strlen(value) == 0)) - errx(1, "missing property name or value"); - - test_args.property = property; - test_args.value = value; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { - do { - result = bson_decoder_next(&decoder); - } while (result == 1); - } else { - warnx("failed to init decoder"); - } - free(buf); - - /* if we matched, we exit with zero success */ - exit((result == 2) ? 0 : 1); -} - -int -boardinfo_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "set")) - boardinfo_set(&eeprom_info[0], argv[2]); - - if (!strcmp(argv[1], "show")) - boardinfo_show(&eeprom_info[0]); - - if (!strcmp(argv[1], "test")) - boardinfo_test(&eeprom_info[0], argv[2], argv[3]); - - errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); -} diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index aee26680cc..d74010553a 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -168,8 +168,13 @@ esc_calib_main(int argc, char *argv[]) } } - if (set_mask == 0) + if (set_mask == 0) { usage("no channels chosen"); + } + + if (pwm_low > pwm_high) { + usage("low pwm is higher than high pwm"); + } /* make sure no other source is publishing control values now */ struct actuator_controls_s actuators; diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index c15e042a77..6b855cf582 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -235,8 +235,9 @@ do_import(const char *param_file_name) static void do_show(const char *search_string) { - printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, (char *)search_string, false); + printf("Symbols: x = used, + = saved, * = unsaved\n"); + param_foreach(do_show_print, (char *)search_string, false, false); + printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); exit(0); } @@ -278,12 +279,12 @@ do_show_print(void *arg, param_t param) } /* the search string must have been consumed */ - if (!(*ss == '\0' || *ss == '*')) { + if (!(*ss == '\0' || *ss == '*') || *pp != '\0') { return; } } - printf("%c %s: ", + printf("%c %c %s: ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index a788dfc11c..4ab92dde6b 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,11 +68,13 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { perf_print_latency(0 /* stdout */); fflush(stdout); return 0; } + printf("Usage: perf [reset | latency]\n"); return -1; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c deleted file mode 100644 index 361ca67545..0000000000 --- a/src/systemcmds/preflight_check/preflight_check.c +++ /dev/null @@ -1,286 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file preflight_check.c - * - * Preflight check for main system components - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -__EXPORT int preflight_check_main(int argc, char *argv[]); -static int led_toggle(int leds, int led); -static int led_on(int leds, int led); -static int led_off(int leds, int led); - -int preflight_check_main(int argc, char *argv[]) -{ - bool fail_on_error = false; - - if (argc > 1 && !strcmp(argv[1], "--help")) { - warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); - exit(1); - } - - if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { - fail_on_error = true; - } - - bool system_ok = true; - - int fd; - /* open text message output path */ - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - int ret; - int32_t devid, calibration_devid; - - /* give the system some time to sample the sensors in the background */ - usleep(150000); - - /* ---- MAG ---- */ - fd = open(MAG0_DEVICE_PATH, 0); - if (fd < 0) { - warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); - system_ok = false; - goto system_eval; - } - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, MAGIOCSELFTEST, 0); - - if (ret != OK) { - warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* ---- ACCEL ---- */ - - close(fd); - fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* check measurement result range */ - struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { - warnx("accel with spurious values"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); - /* this is frickin' fatal */ - fail_on_error = true; - system_ok = false; - goto system_eval; - } - } else { - warnx("accel read failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); - /* this is frickin' fatal */ - fail_on_error = true; - system_ok = false; - goto system_eval; - } - - /* ---- GYRO ---- */ - - close(fd); - fd = open(GYRO0_DEVICE_PATH, 0); - - devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("gyro calibration is for a different device - calibrate gyro first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); - system_ok = false; - goto system_eval; - } - - ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); - system_ok = false; - goto system_eval; - } - - /* ---- BARO ---- */ - - close(fd); - fd = open(BARO0_DEVICE_PATH, 0); - close(fd); - - /* ---- RC CALIBRATION ---- */ - - bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* warn */ - if (!rc_ok) - warnx("rc calibration test failed"); - - /* require RC ok to keep system_ok */ - system_ok &= rc_ok; - - - - -system_eval: - - if (system_ok) { - /* all good, exit silently */ - exit(0); - } else { - fflush(stdout); - - warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); - fflush(stderr); - - int buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - int leds = open(LED0_DEVICE_PATH, 0); - - if (leds < 0) { - close(buzzer); - errx(1, "failed to open leds, aborting"); - } - - /* flip blue led into alternating amber */ - led_off(leds, LED_BLUE); - led_off(leds, LED_AMBER); - led_toggle(leds, LED_BLUE); - - /* display and sound error */ - for (int i = 0; i < 14; i++) - { - led_toggle(leds, LED_BLUE); - led_toggle(leds, LED_AMBER); - - if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); - } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); - } - usleep(100000); - } - - /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - - /* switch off leds */ - led_on(leds, LED_BLUE); - led_on(leds, LED_AMBER); - close(leds); - - if (fail_on_error) { - /* exit with error message */ - exit(1); - } else { - /* do not emit an error code to make sure system still boots */ - exit(0); - } - } -} - -static int led_toggle(int leds, int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_off(int leds, int led) -{ - return ioctl(leds, LED_OFF, led); -} - -static int led_on(int leds, int led) -{ - return ioctl(leds, LED_ON, led); -} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 620b0a6f67..6bb9f235cb 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -68,41 +68,43 @@ __EXPORT int pwm_main(int argc, char *argv[]); static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } + errx(1, - "usage:\n" - "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" - "\n" - "arm\t\t\t\tArm output\n" - "disarm\t\t\t\tDisarm output\n" - "\n" - "rate ...\t\t\tConfigure PWM rates\n" - "\t[-g ]\t(e.g. 0,1,2)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-r \t\tPWM rate (50 to 400 Hz)\n" - "\n" - "failsafe ...\t\t\tFailsafe PWM\n" - "disarmed ...\t\t\tDisarmed PWM\n" - "min ...\t\t\t\tMinimum PWM\n" - "max ...\t\t\t\tMaximum PWM\n" - "\t[-c ]\t\t(e.g. 1234)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-p \t\tPWM value\n" - "\n" - "test ...\t\t\tDirectly set PWM\n" - "\t[-c ]\t\t(e.g. 1234)\n" - "\t[-m ]\t(e.g. 0xF)\n" - "\t[-a]\t\t\tConfigure all outputs\n" - "\t-p \t\tPWM value\n" - "\n" - "info\t\t\t\tPrint information\n" - "\n" - "\t-v\t\t\tVerbose\n" - "\t-d \t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n" - ); + "usage:\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" + "\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" + "\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM rate (50 to 400 Hz)\n" + "\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" + "\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" + "\n" + "info\t\t\t\tPrint information\n" + "\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT0_DEVICE_PATH ")\n" + ); } @@ -123,10 +125,11 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 2) + if (argc < 2) { usage(NULL); + } - while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { + while ((ch = getopt(argc - 1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { case 'd': @@ -134,6 +137,7 @@ pwm_main(int argc, char *argv[]) warnx("device %s not valid", optarg); usage(NULL); } + dev = optarg; break; @@ -147,15 +151,19 @@ pwm_main(int argc, char *argv[]) while ((single_ch = channels % 10)) { - set_mask |= 1<<(single_ch-1); + set_mask |= 1 << (single_ch - 1); channels /= 10; } + break; case 'g': group = strtoul(optarg, &ep, 0); - if ((*ep != '\0') || (group >= 32)) + + if ((*ep != '\0') || (group >= 32)) { usage("bad channel_group value"); + } + alt_channel_groups |= (1 << group); alt_channels_set = true; warnx("alt channels set, group: %d", group); @@ -164,25 +172,38 @@ pwm_main(int argc, char *argv[]) case 'm': /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); - if (*ep != '\0') + + if (*ep != '\0') { usage("BAD set_mask VAL"); + } + break; case 'a': - for (unsigned i = 0; i 0) { warnx("Channels: "); printf(" "); - for (unsigned i = 0; i 0) { ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } } /* directly supplied channel mask */ if (set_mask > 0) { ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } } /* assign alternate rate to channel groups */ @@ -258,17 +301,22 @@ pwm_main(int argc, char *argv[]) uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + } mask |= group_mask; } } ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); - if (ret != OK) + + if (ret != OK) { err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } } + exit(0); } else if (!strcmp(argv[1], "min")) { @@ -276,34 +324,46 @@ pwm_main(int argc, char *argv[]) if (set_mask == 0) { usage("no channels set"); } - if (pwm_value == 0) + + if (pwm_value == 0) { usage("no PWM value provided"); + } struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { errx(ret, "failed get min values"); } for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { - read(0, &c, 1); + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1< 0) { ret = read(0, &c, 1); @@ -546,23 +660,29 @@ pwm_main(int argc, char *argv[]) if (ret > 0) { /* reset output to the last value */ for (unsigned i = 0; i < servo_count; i++) { - if (set_mask & 1<type, BSON_BOOL); return 1; } + if (node->b != sample_bool) { - warnx("FAIL: decoder: bool1 value %s, expected %s", - (node->b ? "true" : "false"), - (sample_bool ? "true" : "false")); + warnx("FAIL: decoder: bool1 value %s, expected %s", + (node->b ? "true" : "false"), + (sample_bool ? "true" : "false")); return 1; } + warnx("PASS: decoder: bool1"); return 1; } + if (!strcmp(node->name, "int1")) { if (node->type != BSON_INT32) { warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32); return 1; } + if (node->i != sample_small_int) { warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int); return 1; } + warnx("PASS: decoder: int1"); return 1; } + if (!strcmp(node->name, "int2")) { if (node->type != BSON_INT64) { warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64); return 1; } + if (node->i != sample_big_int) { warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int); return 1; } + warnx("PASS: decoder: int2"); return 1; } + if (!strcmp(node->name, "double1")) { if (node->type != BSON_DOUBLE) { warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } + if (fabs(node->d - sample_double) > 1e-12) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } + warnx("PASS: decoder: double1"); return 1; } + if (!strcmp(node->name, "string1")) { if (node->type != BSON_STRING) { warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING); @@ -150,21 +173,26 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: string1 copy failed"); return 1; } + if (bson_decoder_data_pending(decoder) != 0) { warnx("FAIL: decoder: string1 copy did not exhaust all data"); return 1; } + if (sbuf[len - 1] != '\0') { warnx("FAIL: decoder: string1 not 0-terminated"); return 1; } + if (strcmp(sbuf, sample_string)) { warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string); return 1; } + warnx("PASS: decoder: string1"); return 1; } + if (!strcmp(node->name, "data1")) { if (node->type != BSON_BINDATA) { warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA); @@ -177,7 +205,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data)); return 1; } - + if (node->subtype != BSON_BIN_BINARY) { warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY); return 1; @@ -189,20 +217,25 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: data1 copy failed"); return 1; } + if (bson_decoder_data_pending(decoder) != 0) { warnx("FAIL: decoder: data1 copy did not exhaust all data"); return 1; } + if (memcmp(sample_data, dbuf, len)) { warnx("FAIL: decoder: data1 compare fail"); return 1; } + warnx("PASS: decoder: data1"); return 1; } - if (node->type != BSON_EOO) + if (node->type != BSON_EOO) { warnx("FAIL: decoder: unexpected node name '%s'", node->name); + } + return 1; } @@ -225,19 +258,28 @@ test_bson(int argc, char *argv[]) int len; /* encode data to a memory buffer */ - if (bson_encoder_init_buf(&encoder, NULL, 0)) + if (bson_encoder_init_buf(&encoder, NULL, 0)) { errx(1, "FAIL: bson_encoder_init_buf"); + } + encode(&encoder); len = bson_encoder_buf_size(&encoder); - if (len <= 0) + + if (len <= 0) { errx(1, "FAIL: bson_encoder_buf_len"); + } + buf = bson_encoder_buf_data(&encoder); - if (buf == NULL) + + if (buf == NULL) { errx(1, "FAIL: bson_encoder_buf_data"); + } /* now test-decode it */ - if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) + if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL)) { errx(1, "FAIL: bson_decoder_init_buf"); + } + decode(&decoder); free(buf); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index fda949c610..9a41b19b95 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -61,11 +61,13 @@ int test_conv(int argc, char *argv[]) { warnx("Testing system conversions"); - for (int i = -10000; i <= 10000; i+=1) { - float f = i/10000.0f; + for (int i = -10000; i <= 10000; i += 1) { + float f = i / 10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), + (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 1f844a97de..c58e1a51ea 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -70,19 +70,23 @@ task_main(int argc, char *argv[]) warnx("Starting dataman test task %s", argv[1]); /* try to read an invalid item */ int my_id = atoi(argv[1]); + /* try to read an invalid item */ if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid item failed", my_id); goto fail; } + /* try to read an invalid index */ if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { warnx("%d read an invalid index failed", my_id); goto fail; } + srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; @@ -91,44 +95,53 @@ task_main(int argc, char *argv[]) int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); + if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } + usleep(rand() & ((64 * 1024) - 1)); } + rstart = hrt_absolute_time(); wend = rstart; for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } + if (buffer[0] == my_id) { hit++; + if (len2 != len) { warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); goto fail; } + if (buffer[1] != i) { warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); goto fail; } - } - else + + } else { miss++; + } } + rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", - my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", - my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); + my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); sem_post(sems + my_id); return -1; } @@ -138,11 +151,13 @@ int test_dataman(int argc, char *argv[]) int i, num_tasks = 4; char buffer[DM_MAX_DATA_SIZE]; - if (argc > 1) + if (argc > 1) { num_tasks = atoi(argv[1]); - + } + sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); warnx("Running %d tasks", num_tasks); + for (i = 0; i < num_tasks; i++) { int task; char a[16]; @@ -151,32 +166,41 @@ int test_dataman(int argc, char *argv[]) av[0] = a; av[1] = 0; sem_init(sems + i, 1, 0); + /* start the task */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { warn("task start failed"); } } + for (i = 0; i < num_tasks; i++) { sem_wait(sems + i); sem_destroy(sems + i); } + free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { break; + } } + if (i >= NUM_MISSIONS_SUPPORTED) { warnx("Restart in-flight failed"); return -1; } + dm_restart(DM_INIT_REASON_POWER_ON); + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } } + return 0; } diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp new file mode 100644 index 0000000000..068e07c388 --- /dev/null +++ b/src/systemcmds/tests/test_eigen.cpp @@ -0,0 +1,430 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_eigen.cpp + * + * Eigen test (based of mathlib test) + * @author Johan Jansen + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +namespace Eigen +{ +typedef Matrix Vector10f; +} + +static constexpr size_t OPERATOR_ITERATIONS = 60000; + +#define TEST_OP(_title, _op) \ + { \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf(_title ": %.6fus\n", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ + } + +#define VERIFY_OP(_title, _op, __OP_TEST__) \ + { \ + _op; \ + if(!(__OP_TEST__)) { \ + printf(_title " Failed! ("#__OP_TEST__")\n"); \ + } \ + } + +#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ + VERIFY_OP(_title, _op, __OP_TEST__) \ + TEST_OP(_title, _op) + +/** +* @brief +* Prints an Eigen::Matrix to stdout +**/ +template +void printEigen(const Eigen::MatrixBase &b) +{ + for (int i = 0; i < b.rows(); ++i) { + printf("("); + + for (int j = 0; j < b.cols(); ++i) { + if (j > 0) { + printf(","); + } + + printf("%.3f", static_cast(b(i, j))); + } + + printf(")%s\n", i + 1 < b.rows() ? "," : ""); + } +} + +/** +* @brief +* Construct new Eigen::Quaternion from euler angles +**/ +template +Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) +{ + Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); + Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); + Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); + return rollAngle * yawAngle * pitchAngle; +} + + +int test_eigen(int argc, char *argv[]) +{ + int rc = 0; + warnx("testing eigen"); + + { + Eigen::Vector2f v; + Eigen::Vector2f v1(1.0f, 2.0f); + Eigen::Vector2f v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); + TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1)); + TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); + TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); + TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1)); + TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); + TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); + //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? + } + + { + Eigen::Vector3f v; + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + Eigen::Vector3f v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3); + TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1)); + TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data)); + TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector3f = Vector3f", v = v1); + TEST_OP("Vector3f + Vector3f", v + v1); + TEST_OP("Vector3f - Vector3f", v - v1); + TEST_OP("Vector3f += Vector3f", v += v1); + TEST_OP("Vector3f -= Vector3f", v -= v1); + TEST_OP("Vector3f * float", v1 * 2.0f); + TEST_OP("Vector3f / float", v1 / 2.0f); + TEST_OP("Vector3f *= float", v1 *= 2.0f); + TEST_OP("Vector3f /= float", v1 /= 2.0f); + TEST_OP("Vector3f dot Vector3f", v.dot(v1)); + TEST_OP("Vector3f cross Vector3f", v1.cross(v2)); + TEST_OP("Vector3f length", v1.norm()); + TEST_OP("Vector3f length squared", v1.squaredNorm()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]); +#pragma GCC diagnostic pop + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f); + } + + { + Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f); + Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); + Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); + Eigen::Vector4f vres; + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + //TEST_OP("Vector<4> += Vector<4>", v += v1); + //TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); + } + + { + Eigen::Vector10f v1; + v1.Zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + } + + { + Eigen::Matrix3f m1; + m1.setIdentity(); + Eigen::Matrix3f m2; + m2.setIdentity(); + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix3f * Vector3f", m1 * v1); + TEST_OP("Matrix3f + Matrix3f", m1 + m2); + TEST_OP("Matrix3f * Matrix3f", m1 * m2); + } + + { + Eigen::Matrix m1; + m1.setIdentity(); + Eigen::Matrix m2; + m2.setIdentity(); + Eigen::Vector10f v1; + v1.Zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + { + warnx("Nonsymmetric matrix operations test"); + // test nonsymmetric +, -, +=, -= + + const Eigen::Matrix m1_orig = + (Eigen::Matrix() << 1, 3, 3, + 4, 6, 6).finished(); + + Eigen::Matrix m1(m1_orig); + + Eigen::Matrix m2; + m2 << 2, 4, 6, + 8, 10, 12; + + Eigen::Matrix m3; + m3 << 3, 6, 9, + 12, 15, 18; + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printEigen(m1 + m2); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printEigen(m3 - m2); + printf("!=\n"); + printEigen(m1); + rc = 1; + } + + m1 += m2; + + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + m1 -= m2; + + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m1_orig); + rc = 1; + } + + } + + { + // test conversion rotation matrix to quaternion and back + Eigen::Matrix3f R_orig; + Eigen::Matrix3f R; + Eigen::Quaternionf q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.eulerAngles(roll, pitch, yaw); + q = R_orig; //from_dcm + R = q.toRotationMatrix(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.setIdentity(); + q = R_orig; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + } + + { + // test quaternion method "rotate" (rotate vector by quaternion) + Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f}; + Eigen::Vector3f vector_q; + Eigen::Vector3f vector_r; + Eigen::Quaternionf q; + Eigen::Matrix3f R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion vector rotation method test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R.eulerAngles(roll, pitch, yaw); + q = quatFromEuler(roll, pitch, yaw); + vector_r = R * vector; + vector_q = q._transformVector(vector); + + for (int i = 0; i < 3; i++) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f); + vector_q = q._transformVector(vector); + Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(0.3f, 0.2f, 0.1f); + vector_q = q._transformVector(vector); + vector_true = {1.1566, 0.7792, 1.0273}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(-1.5f, -0.2f, 0.5f); + vector_q = q._transformVector(vector); + vector_true = {0.5095, 1.4956, -0.7096}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); + vector_q = q._transformVector(vector); + vector_true = { -1.3660, 0.3660, 1.0000}; + + for (size_t i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + return rc; +} \ No newline at end of file diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 570583dee9..a43e01d6fb 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -56,7 +56,8 @@ static int check_user_abort(int fd); -int check_user_abort(int fd) { +int check_user_abort(int fd) +{ /* check if user wants to abort */ char c; @@ -74,13 +75,12 @@ int check_user_abort(int fd) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': - case 'q': - { - warnx("Test aborted."); - fsync(fd); - close(fd); - return OK; - /* not reached */ + case 'q': { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ } } } @@ -96,6 +96,7 @@ test_file(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; @@ -118,7 +119,7 @@ test_file(int argc, char *argv[]) /* fill write buffer with known values */ for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; + write_buf[i] = i + 11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -129,24 +130,28 @@ test_file(int argc, char *argv[]) warnx("testing unaligned writes - please wait.."); start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + a))) + if ((0x3 & (uintptr_t)(write_buf + a))) { warnx("memory is unaligned, align shift: %d", a); + } return 1; } fsync(fd); - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } + end = hrt_absolute_time(); warnx("write took %llu us", (end - start)); @@ -162,7 +167,7 @@ test_file(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + /* compare value */ bool compare_ok = true; @@ -179,8 +184,9 @@ test_file(int argc, char *argv[]) return 1; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } @@ -202,8 +208,9 @@ test_file(int argc, char *argv[]) return 1; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } @@ -224,7 +231,7 @@ test_file(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); @@ -232,8 +239,9 @@ test_file(int argc, char *argv[]) break; } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } if (!align_read_ok) { @@ -267,16 +275,19 @@ test_file(int argc, char *argv[]) for (int j = 0; j < chunk_sizes[c]; j++) { if ((read_buf + a)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], + (unsigned int)write_buf[j]); unalign_read_ok = false; unalign_read_err_count++; - - if (unalign_read_err_count > 10) + + if (unalign_read_err_count > 10) { break; + } } - if (!check_user_abort(fd)) + if (!check_user_abort(fd)) { return OK; + } } if (!unalign_read_ok) { @@ -300,6 +311,7 @@ test_file(int argc, char *argv[]) DIR *d; struct dirent *dir; d = opendir("/fs/microsd"); + if (d) { while ((dir = readdir(d)) != NULL) { diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 8db3ea5ae6..2e7b5c184f 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -69,42 +69,51 @@ static uint8_t get_value(uint32_t ofs) static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) { - printf("Testing on %s with write_chunk=%u write_size=%u\n", + printf("Testing on %s with write_chunk=%u write_size=%u\n", filename, (unsigned)write_chunk, (unsigned)write_size); - + uint32_t ofs = 0; int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { perror(filename); exit(1); } - + // create a file of size write_size, in write_chunk blocks uint8_t counter = 0; + while (ofs < write_size) { - uint8_t buffer[write_chunk]; - for (uint16_t j=0; j= 0) + if (fd >= 0) { close(fd); + } return 0; } diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 98a105cb3e..554125302a 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -162,7 +162,7 @@ int test_jig_voltages(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return ret; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index a66cebd2f4..3a890c30b6 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -284,11 +284,11 @@ int test_mathlib(int argc, char *argv[]) { // test quaternion method "rotate" (rotate vector by quaternion) - Vector<3> vector = {1.0f,1.0f,1.0f}; + Vector<3> vector = {1.0f, 1.0f, 1.0f}; Vector<3> vector_q; Vector<3> vector_r; Quaternion q; - Matrix<3,3> R; + Matrix<3, 3> R; float diff = 0.1f; float tol = 0.00001f; @@ -298,11 +298,12 @@ int test_mathlib(int argc, char *argv[]) for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { R.from_euler(roll, pitch, yaw); - q.from_euler(roll,pitch,yaw); - vector_r = R*vector; + q.from_euler(roll, pitch, yaw); + vector_r = R * vector; vector_q = q.rotate(vector); + for (int i = 0; i < 3; i++) { - if(fabsf(vector_r(i) - vector_q(i)) > tol) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } @@ -313,41 +314,45 @@ int test_mathlib(int argc, char *argv[]) // test some values calculated with matlab tol = 0.0001f; - q.from_euler(M_PI_2_F,0.0f,0.0f); + q.from_euler(M_PI_2_F, 0.0f, 0.0f); vector_q = q.rotate(vector); - Vector<3> vector_true = {1.00f,-1.00f,1.00f}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + Vector<3> vector_true = {1.00f, -1.00f, 1.00f}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(0.3f,0.2f,0.1f); + q.from_euler(0.3f, 0.2f, 0.1f); vector_q = q.rotate(vector); - vector_true = {1.1566,0.7792,1.0273}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {1.1566, 0.7792, 1.0273}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(-1.5f,-0.2f,0.5f); + q.from_euler(-1.5f, -0.2f, 0.5f); vector_q = q.rotate(vector); - vector_true = {0.5095,1.4956,-0.7096}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = {0.5095, 1.4956, -0.7096}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } } - q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); vector_q = q.rotate(vector); - vector_true = {-1.3660,0.3660,1.0000}; - for(unsigned i = 0;i<3;i++) { - if(fabsf(vector_true(i) - vector_q(i)) > tol) { + vector_true = { -1.3660, 0.3660, 1.0000}; + + for (unsigned i = 0; i < 3; i++) { + if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); rc = 1; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 2896a8e405..19aa059707 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -83,8 +83,9 @@ int test_mixer(int argc, char *argv[]) const char *filename = "/etc/mixers/IO_pass.mix"; - if (argc > 1) + if (argc > 1) { filename = argv[1]; + } warnx("loading: %s", filename); @@ -108,8 +109,10 @@ int test_mixer(int argc, char *argv[]) unsigned xx = loaded; mixer_group.load_from_buf(&buf[0], xx); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + + if (mixer_group.count() != 8) { return 1; + } unsigned empty_load = 2; char empty_buf[2]; @@ -118,8 +121,10 @@ int test_mixer(int argc, char *argv[]) mixer_group.reset(); mixer_group.load_from_buf(&empty_buf[0], empty_load); warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); - if (empty_load != 0) + + if (empty_load != 0) { return 1; + } /* FIRST mark the mixer as invalid */ /* THEN actually delete it */ @@ -155,8 +160,9 @@ int test_mixer(int argc, char *argv[]) warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) + if (resid > 0) { memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + } mixer_text_length = resid; } @@ -166,11 +172,12 @@ int test_mixer(int argc, char *argv[]) warnx("chunked load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 8) { return 1; + } /* execute the mixer */ - + float outputs[output_max]; unsigned mixed; const int jmax = 5; @@ -193,30 +200,31 @@ int test_mixer(int argc, char *argv[]) unsigned sleepcount = 0; while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { - - /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && - r_page_servos[i] != r_page_servo_disarmed[i]) { + r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); return 1; } if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && - r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { warnx("ramp servo value mismatch"); return 1; } //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -225,6 +233,7 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); warnx("ARMING TEST: NORMAL OPERATION"); @@ -232,21 +241,23 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { for (unsigned i = 0; i < output_max; i++) { - actuator_controls[i] = j/10.0f + 0.1f * i; + actuator_controls[i] = j / 10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); + mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); @@ -262,15 +273,15 @@ int test_mixer(int argc, char *argv[]) should_arm = false; while (hrt_elapsed_time(&starttime) < 600000) { - - /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); @@ -279,6 +290,7 @@ int test_mixer(int argc, char *argv[]) //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -287,6 +299,7 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); warnx("ARMING TEST: REARMING: STARTING RAMP"); @@ -296,30 +309,30 @@ int test_mixer(int argc, char *argv[]) should_arm = true; while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { - - /* mix */ - mixed = mixer_group.mix(&outputs[0], output_max); - pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (unsigned i = 0; i < mixed; i++) - { + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; /* check ramp */ if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && - (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || - r_page_servos[i] > servo_predicted[i])) { + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { warnx("ramp servo value mismatch"); return 1; } /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && - fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; @@ -327,6 +340,7 @@ int test_mixer(int argc, char *argv[]) //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } + usleep(sleep_quantum_us); sleepcount++; @@ -335,15 +349,18 @@ int test_mixer(int argc, char *argv[]) fflush(stdout); } } + printf("\n"); /* load multirotor at once test */ mixer_group.reset(); - if (argc > 2) + if (argc > 2) { filename = argv[2]; - else + + } else { filename = "/etc/mixers/quad_w.main.mix"; + } load_mixer_file(filename, &buf[0], sizeof(buf)); loaded = strlen(buf); @@ -353,6 +370,7 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 5) { warnx("FAIL: Quad W mixer load failed"); return 1; @@ -368,11 +386,13 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group != 0) { return -1; + } - if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) { return -1; + } control = actuator_controls[control_index]; diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 4b6303cfb1..68052258ef 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -63,11 +63,12 @@ test_mount(int argc, char *argv[]) const unsigned iterations = 2000; const unsigned alignments = 10; - const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + const char *cmd_filename = "/fs/microsd/mount_test_cmds.txt"; /* check if microSD card is mounted */ struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { warnx("no microSD card mounted, aborting file test"); return 1; @@ -77,6 +78,7 @@ test_mount(int argc, char *argv[]) DIR *d; struct dirent *dir; d = opendir("/fs/microsd"); + if (d) { while ((dir = readdir(d)) != NULL) { @@ -105,6 +107,7 @@ test_mount(int argc, char *argv[]) int it_left_abort = abort_tries; int cmd_fd; + if (stat(cmd_filename, &buffer) == OK) { /* command file exists, read off state */ @@ -115,24 +118,28 @@ test_mount(int argc, char *argv[]) if (ret > 0) { int count = 0; ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { buf[0] = '\0'; } - if (it_left_fsync > fsync_tries) + if (it_left_fsync > fsync_tries) { it_left_fsync = fsync_tries; + } - if (it_left_abort > abort_tries) + if (it_left_abort > abort_tries) { it_left_abort = abort_tries; + } warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, - fsync_tries, abort_tries, buf); + fsync_tries, abort_tries, buf); int it_left_fsync_prev = it_left_fsync; /* now write again what to do next */ - if (it_left_fsync > 0) + if (it_left_fsync > 0) { it_left_fsync--; + } if (it_left_fsync == 0 && it_left_abort > 0) { @@ -172,7 +179,8 @@ test_mount(int argc, char *argv[]) for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], + (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); fsync(fileno(stdout)); fsync(fileno(stderr)); @@ -187,7 +195,7 @@ test_mount(int argc, char *argv[]) /* fill write buffer with known values */ for (unsigned i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; + write_buf[i] = i + 11; } uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -201,8 +209,9 @@ test_mount(int argc, char *argv[]) if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + a))) + if ((0x3 & (uintptr_t)(write_buf + a))) { warnx("memory is unaligned, align shift: %d", a); + } return 1; @@ -210,6 +219,7 @@ test_mount(int argc, char *argv[]) if (it_left_fsync > 0) { fsync(fd); + } else { printf("#"); fsync(fileno(stdout)); @@ -237,7 +247,7 @@ test_mount(int argc, char *argv[]) warnx("READ ERROR!"); return 1; } - + /* compare value */ bool compare_ok = true; diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index a753f45c23..c802cdf0c6 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -77,6 +77,7 @@ int test_ppm_loopback(int argc, char *argv[]) unsigned servo_count; result = ioctl(servo_fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); return ERROR; @@ -97,7 +98,7 @@ int test_ppm_loopback(int argc, char *argv[]) // result = ioctl(servo_fd, PWM_SERVO_SET_ARM_OK, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_SET_ARM_OK"); - // tell output device that the system is armed (it will output values if safety is off) + // tell output device that the system is armed (it will output values if safety is off) // result = ioctl(servo_fd, PWM_SERVO_ARM, 0); // if (result != OK) // warnx("FAIL: PWM_SERVO_ARM"); @@ -105,16 +106,17 @@ int test_ppm_loopback(int argc, char *argv[]) int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - // if (result) { - // (void)close(servo_fd); - // return ERROR; - // } else { - // warnx("channel %d set to %d", i, pwm_values[i]); - // } - // } + if (result) { + (void)close(servo_fd); + return ERROR; + + } else { + warnx("channel %d set to %d", i, pwm_values[i]); + } + } warnx("servo count: %d", servo_count); @@ -166,6 +168,7 @@ int test_ppm_loopback(int argc, char *argv[]) return ERROR; } } + } else { warnx("failed reading RC input data"); return ERROR; diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index c9f9ef4398..3a8144077c 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -126,7 +126,7 @@ int test_rc(int argc, char *argv[]) warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; - } + } } else { /* key pressed, bye bye */ diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index f73f8b87a4..b6d660c836 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -34,7 +34,7 @@ /** * @file test_sensors.c * Tests the onboard sensors. - * + * * @author Lorenz Meier */ @@ -62,10 +62,10 @@ #include #include -static int accel(int argc, char *argv[], const char* path); -static int gyro(int argc, char *argv[], const char* path); -static int mag(int argc, char *argv[], const char* path); -static int baro(int argc, char *argv[], const char* path); +static int accel(int argc, char *argv[], const char *path); +static int gyro(int argc, char *argv[], const char *path); +static int mag(int argc, char *argv[], const char *path); +static int baro(int argc, char *argv[], const char *path); /**************************************************************************** * Private Data @@ -74,7 +74,7 @@ static int baro(int argc, char *argv[], const char* path); struct { const char *name; const char *path; - int (* test)(int argc, char *argv[], const char* path); + int (* test)(int argc, char *argv[], const char *path); } sensors[] = { {"accel0", ACCEL0_DEVICE_PATH, accel}, {"accel1", ACCEL1_DEVICE_PATH, accel}, @@ -86,7 +86,7 @@ struct { }; static int -accel(int argc, char *argv[], const char* path) +accel(int argc, char *argv[], const char *path) { printf("\tACCEL: test start\n"); fflush(stdout); @@ -136,7 +136,7 @@ accel(int argc, char *argv[], const char* path) } static int -gyro(int argc, char *argv[], const char* path) +gyro(int argc, char *argv[], const char *path) { printf("\tGYRO: test start\n"); fflush(stdout); @@ -181,7 +181,7 @@ gyro(int argc, char *argv[], const char* path) } static int -mag(int argc, char *argv[], const char* path) +mag(int argc, char *argv[], const char *path) { printf("\tMAG: test start\n"); fflush(stdout); @@ -226,7 +226,7 @@ mag(int argc, char *argv[], const char* path) } static int -baro(int argc, char *argv[], const char* path) +baro(int argc, char *argv[], const char *path) { printf("\tBARO: test start\n"); fflush(stdout); @@ -253,7 +253,8 @@ baro(int argc, char *argv[], const char* path) return ERROR; } else { - printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); + printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, + (double)buf.temperature); } /* Let user know everything is ok */ diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ba68485224..9ffd83c65f 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -81,6 +81,7 @@ int test_servo(int argc, char *argv[]) unsigned servo_count; result = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (result != OK) { warnx("PWM_SERVO_GET_COUNT"); return ERROR; @@ -100,12 +101,17 @@ int test_servo(int argc, char *argv[]) /* tell safety that its ok to disable it with the switch */ result = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (result != OK) + + if (result != OK) { warnx("FAIL: PWM_SERVO_SET_ARM_OK"); + } + /* tell output device that the system is armed (it will output values if safety is off) */ result = ioctl(fd, PWM_SERVO_ARM, 0); - if (result != OK) + + if (result != OK) { warnx("FAIL: PWM_SERVO_ARM"); + } usleep(5000000); printf("Advancing channel 0 to 1500\n"); diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 8a164f3fc2..78e13a2420 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -90,8 +90,9 @@ cycletime(void) cycles = *(unsigned long *)0xe0001004; - if (cycles < lasttime) + if (cycles < lasttime) { basetime += 0x100000000ULL; + } lasttime = cycles; @@ -147,11 +148,13 @@ int test_time(int argc, char *argv[]) delta = abs(h - c); deltadelta = abs(delta - lowdelta); - if (deltadelta > maxdelta) + if (deltadelta > maxdelta) { maxdelta = deltadelta; + } - if (deltadelta > 1000) + if (deltadelta > 1000) { fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + } } printf("Maximum jitter %lldus\n", maxdelta); diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 609a65c624..cd2e561313 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -146,8 +146,9 @@ int test_uart_baudchange(int argc, char *argv[]) /* uart2 -> */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart2_nwrite += r; + } } close(uart2); diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index f1d41739be..6076dbdeae 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -92,8 +92,9 @@ int test_uart_loopback(int argc, char *argv[]) /* uart2 -> uart5 */ r = write(uart2, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart2_nwrite += r; + } // printf("TEST #%d\n",i); write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); @@ -101,8 +102,9 @@ int test_uart_loopback(int argc, char *argv[]) /* uart2 -> uart5 */ r = write(uart5, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart5_nwrite += r; + } // printf("TEST #%d\n",i); write(stdout_fd, sample_stdout_fd, sizeof(sample_stdout_fd)); @@ -111,8 +113,9 @@ int test_uart_loopback(int argc, char *argv[]) do { r = read(uart5, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart5_nread += r; + } } while (r > 0); // printf("TEST #%d\n",i); @@ -121,8 +124,9 @@ int test_uart_loopback(int argc, char *argv[]) do { r = read(uart2, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart2_nread += r; + } } while (r > 0); // printf("TEST #%d\n",i); @@ -134,16 +138,19 @@ int test_uart_loopback(int argc, char *argv[]) /* try to read back values */ r = read(uart5, sample_uart2, sizeof(sample_uart2)); - if (r > 0) + if (r > 0) { uart5_nread += r; + } r = read(uart2, sample_uart5, sizeof(sample_uart5)); - if (r > 0) + if (r > 0) { uart2_nread += r; + } - if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) + if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) { break; + } } diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 7e1e8d307d..70a9c719e1 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -95,7 +95,7 @@ int test_uart_send(int argc, char *argv[]) /* input handling */ char *uart_name = "/dev/ttyS3"; - if (argc > 1) uart_name = argv[1]; + if (argc > 1) { uart_name = argv[1]; } /* assuming NuttShell is on UART1 (/dev/ttyS0) */ int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 69fb0e57bd..9d13d50d77 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -113,6 +113,7 @@ extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); +extern int test_eigen(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0768c1d5aa..55d53503a8 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif + {"eigen", test_eigen, OPT_NOALLTEST | OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; @@ -125,8 +126,9 @@ test_help(int argc, char *argv[]) printf("Available tests:\n"); - for (i = 0; tests[i].name; i++) + for (i = 0; tests[i].name; i++) { printf(" %s\n", tests[i].name); + } return 0; } @@ -154,11 +156,13 @@ test_all(int argc, char *argv[]) fflush(stderr); failcount++; passed[i] = false; + } else { printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); passed[i] = true; } + testcount++; } } @@ -197,7 +201,7 @@ test_all(int argc, char *argv[]) printf("\n"); /* Print failed tests */ - if (failcount > 0) printf(" Failed tests:\n\n"); + if (failcount > 0) { printf(" Failed tests:\n\n"); } unsigned int k; @@ -253,22 +257,26 @@ int test_jig(int argc, char *argv[]) bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); + for (i = 0; tests[i].name; i++) { /* Only run tests that are not excluded */ if (!(tests[i].options & OPT_NOJIGTEST)) { printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); fflush(stdout); + /* Execute test */ if (tests[i].fn(1, args) != 0) { fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); fflush(stderr); failcount++; passed[i] = false; + } else { printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); passed[i] = true; } + testcount++; } } @@ -276,13 +284,15 @@ int test_jig(int argc, char *argv[]) /* Print summary */ printf("\n"); int j; - for (j = 0; j < 40; j++) - { + + for (j = 0; j < 40; j++) { printf("-"); } + printf("\n\n"); printf(" T E S T S U M M A R Y\n\n"); + if (failcount == 0) { printf(" ______ __ __ ______ __ __ \n"); printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); @@ -291,6 +301,7 @@ int test_jig(int argc, char *argv[]) printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); printf("\n"); printf(" All tests passed (%d of %d)\n", testcount, testcount); + } else { printf(" ______ ______ __ __ \n"); printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); @@ -300,18 +311,20 @@ int test_jig(int argc, char *argv[]) printf("\n"); printf(" Some tests failed (%d of %d)\n", failcount, testcount); } + printf("\n"); /* Print failed tests */ - if (failcount > 0) printf(" Failed tests:\n\n"); + if (failcount > 0) { printf(" Failed tests:\n\n"); } + unsigned int k; - for (k = 0; k < i; k++) - { - if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) - { + + for (k = 0; k < i; k++) { + if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) { printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); } } + fflush(stdout); return 0; @@ -332,8 +345,9 @@ int tests_main(int argc, char *argv[]) } for (i = 0; tests[i].name; i++) { - if (!strcmp(tests[i].name, argv[1])) + if (!strcmp(tests[i].name, argv[1])) { return tests[i].fn(argc - 1, argv + 1); + } } printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 98bbbc4be9..530fee3405 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]); int usb_connected_main(int argc, char *argv[]) { - return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; } diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk index 2eeb80b616..4597b5f110 100644 --- a/src/systemcmds/ver/module.mk +++ b/src/systemcmds/ver/module.mk @@ -42,3 +42,5 @@ SRCS = ver.c MODULE_STACKSIZE = 1024 MAXOPTIMIZATION = -Os + +EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 087eb52e3d..b794e8b2fa 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -101,7 +101,7 @@ int ver_main(int argc, char *argv[]) } if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("FW git-hash: %s\n", FW_GIT); + printf("FW git-hash: %s\n", GIT_VERSION); ret = 0; } diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index a27d5f1008..8c89243f6e 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include @@ -12,6 +12,7 @@ #include "gtest/gtest.h" -TEST(AutoDeclinationTest, AutoDeclination) { +TEST(AutoDeclinationTest, AutoDeclination) +{ ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree"; } diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp index 12d2213e2c..7a2909dc34 100644 --- a/unittests/conversion_test.cpp +++ b/unittests/conversion_test.cpp @@ -4,6 +4,7 @@ #include "gtest/gtest.h" -TEST(ConversionTest, quad_w_main) { - ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; +TEST(ConversionTest, quad_w_main) +{ + ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; } diff --git a/unittests/hrt.cpp b/unittests/hrt.cpp index 01b5958b7c..d7b4670db1 100644 --- a/unittests/hrt.cpp +++ b/unittests/hrt.cpp @@ -3,14 +3,16 @@ #include #include -hrt_abstime hrt_absolute_time() { - struct timeval te; - gettimeofday(&te, NULL); // get current time - hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us - return us; +hrt_abstime hrt_absolute_time() +{ + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; } -hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ // not thread safe - return hrt_absolute_time() - *then; + return hrt_absolute_time() - *then; } diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 1271dab5e0..5be4cb032b 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -5,7 +5,8 @@ #include "gtest/gtest.h" -TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; +TEST(MixerTest, Mixer) +{ + char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 5ea6bcc602..44ae4df068 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -13,7 +13,8 @@ struct param_info_s *param_info_limit; /* * Adds test parameters */ -void _add_parameters() { +void _add_parameters() +{ struct param_info_s test_1 = { "TEST_1", PARAM_TYPE_INT32 @@ -44,17 +45,19 @@ void _add_parameters() { param_array[3] = rc2_x; param_info_base = (struct param_info_s *) ¶m_array[0]; param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 + // therefore number of params + 1 } -void _assert_parameter_int_value(param_t param, int32_t expected) { +void _assert_parameter_int_value(param_t param, int32_t expected) +{ int32_t value; int result = param_get(param, &value); ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); } -void _set_all_int_parameters_to(int32_t value) { +void _set_all_int_parameters_to(int32_t value) +{ param_set((param_t)0, &value); param_set((param_t)1, &value); param_set((param_t)2, &value); @@ -66,9 +69,10 @@ void _set_all_int_parameters_to(int32_t value) { _assert_parameter_int_value((param_t)3, value); } -TEST(ParamTest, SimpleFind) { +TEST(ParamTest, SimpleFind) +{ _add_parameters(); - + param_t param = param_find("TEST_2"); ASSERT_NE(PARAM_INVALID, param) << "param_find did not find parameter"; @@ -78,7 +82,8 @@ TEST(ParamTest, SimpleFind) { ASSERT_EQ(4, value) << "value of returned parameter does not match"; } -TEST(ParamTest, ResetAll) { +TEST(ParamTest, ResetAll) +{ _add_parameters(); _set_all_int_parameters_to(50); @@ -90,11 +95,12 @@ TEST(ParamTest, ResetAll) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesOne) { +TEST(ParamTest, ResetAllExcludesOne) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X"}; + const char *excludes[] = {"RC_X"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -103,11 +109,12 @@ TEST(ParamTest, ResetAllExcludesOne) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesTwo) { +TEST(ParamTest, ResetAllExcludesTwo) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 2); _assert_parameter_int_value((param_t)0, 50); @@ -116,11 +123,12 @@ TEST(ParamTest, ResetAllExcludesTwo) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesBoundaryCheck) { +TEST(ParamTest, ResetAllExcludesBoundaryCheck) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -129,11 +137,12 @@ TEST(ParamTest, ResetAllExcludesBoundaryCheck) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesWildcard) { +TEST(ParamTest, ResetAllExcludesWildcard) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC*"}; + const char *excludes[] = {"RC*"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); diff --git a/unittests/queue.h b/unittests/queue.h index 0fdb170db6..e056bc263e 100644 --- a/unittests/queue.h +++ b/unittests/queue.h @@ -67,30 +67,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -110,11 +106,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 15a21c86b7..41f49627d4 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -10,11 +10,12 @@ #include "gtest/gtest.h" -TEST(SBUS2Test, SBUS2) { +TEST(SBUS2Test, SBUS2) +{ const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; - fp = fopen(filepath,"rt"); + fp = fopen(filepath, "rt"); ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); @@ -51,8 +52,9 @@ TEST(SBUS2Test, SBUS2) { //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - if (partial_frame_count == sizeof(frame)) + if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; + } last_time = f; @@ -60,7 +62,7 @@ TEST(SBUS2Test, SBUS2) { hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } ASSERT_EQ(ret, EOF); diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index b161f1ffd0..56e673a06d 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -9,7 +9,8 @@ #include "gtest/gtest.h" -TEST(SF0XTest, SF0X) { +TEST(SF0XTest, SF0X) +{ const char _LINE_MAX = 20; char _linebuf[_LINE_MAX]; _linebuf[0] = '\0'; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 89c7ffb1c3..42fa07ae74 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(ST24Test, ST24) { - const char* filepath = "testdata/st24_data.txt"; +TEST(ST24Test, ST24) +{ + const char *filepath = "testdata/st24_data.txt"; warnx("loading data from: %s", filepath); diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index a919de31b7..bf99709475 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(SUMDTest, SUMD) { - const char* filepath = "testdata/sumd_data.txt"; +TEST(SUMDTest, SUMD) +{ + const char *filepath = "testdata/sumd_data.txt"; warnx("loading data from: %s", filepath); diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp index fc039a4086..e4269afc32 100644 --- a/unittests/uorb_stub.cpp +++ b/unittests/uorb_stub.cpp @@ -10,11 +10,13 @@ * TODO: use googlemock ******************************************/ -orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ return (orb_advert_t)0; } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ return 0; }