diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 00000000..e7488912 --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,25 @@ +name: Documentation + +on: [push] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - name: Set up Python 3.x + uses: actions/setup-python@v1 + with: + python-version: '3.x' + architecture: 'x64' + - name: Display Python version + run: python -c "import sys; print(sys.version)" + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install mkdocs mkdocs-material pygments pymdown-extensions + - name: Build Documentation + run: | + mkdocs build -s diff --git a/.github/workflows/f1_firmware.yml b/.github/workflows/f1_firmware.yml new file mode 100644 index 00000000..0ab3d4fb --- /dev/null +++ b/.github/workflows/f1_firmware.yml @@ -0,0 +1,21 @@ +name: F1 Firmware + +on: [push] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - name: checkout submodules + run: git submodule update --init --recursive + - name: install toolchain + run: | + sudo add-apt-repository -y -u ppa:team-gcc-arm-embedded/ppa + sudo apt -y install gcc-arm-embedded + - name: check toolchain + run: arm-none-eabi-gcc --version + - name: make + run: make BOARD=NAZE -j4 -l4 diff --git a/.github/workflows/f4_firmware.yml b/.github/workflows/f4_firmware.yml new file mode 100644 index 00000000..4a4bd174 --- /dev/null +++ b/.github/workflows/f4_firmware.yml @@ -0,0 +1,21 @@ +name: F4 Firmware + +on: [push] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - name: checkout submodules + run: git submodule update --init --recursive + - name: install toolchain + run: | + sudo add-apt-repository -y -u ppa:team-gcc-arm-embedded/ppa + sudo apt -y install gcc-arm-embedded + - name: check toolchain + run: arm-none-eabi-gcc --version + - name: make + run: make BOARD=REVO -j4 -l4 diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml new file mode 100644 index 00000000..92e49e71 --- /dev/null +++ b/.github/workflows/pre-release.yml @@ -0,0 +1,37 @@ +--- +name: "pre-release" + +on: + push: + branches: + - "master" + +jobs: + pre-release: + name: "Pre Release" + runs-on: "ubuntu-latest" + + steps: + - uses: actions/checkout@v2 + - name: checkout submodules + run: git submodule update --init --recursive + - name: install toolchain + run: | + sudo add-apt-repository -y -u ppa:team-gcc-arm-embedded/ppa + sudo apt -y install gcc-arm-embedded + - name: check toolchain + run: arm-none-eabi-gcc --version + - name: make_f4 + run: make BOARD=REVO -j4 -l4 + - name: make_f1 + run: make BOARD=NAZE -j4 -l4 + + - uses: "marvinpinto/action-automatic-releases@latest" + with: + repo_token: "${{ secrets.GITHUB_TOKEN }}" + automatic_release_tag: "latest" + prerelease: true + title: "Development Build" + files: | + boards/airbourne/build/rosflight_REVO_Release.bin + boards/breezy/build/rosflight_NAZE_Release.hex diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 00000000..22ae3361 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,35 @@ +--- +name: "Release" + +on: + push: + tags: + - 'v*' # Push events to matching v*, i.e. v1.0, v20.15.10 + +jobs: + release: + name: "Release" + runs-on: "ubuntu-latest" + + steps: + - uses: actions/checkout@v2 + - name: checkout submodules + run: git submodule update --init --recursive + - name: install toolchain + run: | + sudo add-apt-repository -y -u ppa:team-gcc-arm-embedded/ppa + sudo apt -y install gcc-arm-embedded + - name: check toolchain + run: arm-none-eabi-gcc --version + - name: make_f4 + run: make BOARD=REVO -j4 -l4 + - name: make_f1 + run: make BOARD=NAZE -j4 -l4 + + - uses: "marvinpinto/action-automatic-releases@latest" + with: + repo_token: "${{ secrets.GITHUB_TOKEN }}" + prerelease: false + files: | + boards/airbourne/build/rosflight_REVO_Release.bin + boards/breezy/build/rosflight_NAZE_Release.hex diff --git a/.github/workflows/unit_tests.yml b/.github/workflows/unit_tests.yml new file mode 100644 index 00000000..727c10cd --- /dev/null +++ b/.github/workflows/unit_tests.yml @@ -0,0 +1,35 @@ +name: Unit Tests + +on: [push] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - name: clone + run: git submodule update --init --recursive + - name: apt install + run: sudo apt-get install -y build-essential cmake libgtest-dev libeigen3-dev + - name: install gtest + run: | + cd /usr/src/gtest + sudo cmake CMakeLists.txt + sudo make + sudo cp *.a /usr/lib + - name: cmake + run: | + cd test + mkdir build + cd build + cmake .. -DCMAKE_BUILD_TYPE=Release + - name: make + run: | + cd test/build + make + - name: test + run: | + cd test/build + ./unit_tests diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index d27ab4af..00000000 --- a/.travis.yml +++ /dev/null @@ -1,39 +0,0 @@ -dist: "xenial" -language: cpp - -addons: - apt: - packages: - - build-essential - - libc6-i386 - - libbz2-1.0:i386 - - libncurses5:i386 - - libz1:i386 - - cmake - - libgtest-dev - - libeigen3-dev - -before_install: - - sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 762E3157 - - sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test - - sudo apt update -qq - - gcc --version - - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q1-update/+download/gcc-arm-none-eabi-5_3-2016q1-20160330-linux.tar.bz2" | tar xfj - - -install: - - sudo pip install mkdocs mkdocs-material pygments pymdown-extensions - - sudo apt install -qq g++-5 gcc-5 - - sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-5 90 - - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 90 - - export PATH=$PATH:$PWD/gcc-arm-none-eabi-5_3-2016q1/bin - - cd /usr/src/gtest - - sudo cmake CMakeLists.txt - - sudo make - - sudo cp *.a /usr/lib - -before_script: - - cd "${TRAVIS_BUILD_DIR}" - - arm-none-eabi-gcc --version - -script: - - ./scripts/run_tests.sh diff --git a/README.md b/README.md index becfbcd9..99862e9e 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,19 @@ # ROSflight -[![Build Status](https://travis-ci.org/rosflight/firmware.svg?branch=master)](https://travis-ci.org/rosflight/firmware) + +![Unit Tests](https://github.com/rosflight/firmware/workflows/Unit%20Tests/badge.svg) +![F4 Firmware](https://github.com/rosflight/firmware/workflows/F4%20Firmware/badge.svg) +![F1 Firmware](https://github.com/rosflight/firmware/workflows/F1%20Firmware/badge.svg) + +![Documentation](https://github.com/rosflight/firmware/workflows/Documentation/badge.svg) This is the firmware required for STM32F10x-based flight controllers (Naze32, Flip32 etc...) and STM32F4x5 boards (Revo) to run ROSflight. ROSflight is a software architecture which uses a simple, inexpensive flight controller in tandem with a much more capable companion computer running ROS. The companion computer is given a high-bandwidth connection to the flight controller to access sensor information and perform actuator commands at high rates. This architecture provides direct control of lower-level functions via the embedded processor while also enabling more complicated functionality such as vision processing and optimization via the ROS middleware. ROSflight is designed to accomplish the following objectives: 1. Provide simpler and easier methods to develop and run advanced autopilot code on both multirotor and fixed-wing UAVs without extensive embedded programming. -1. Robust software-in-the-loop (SIL) simulation tools for rapid testing and development of UAV code. -1. The extensive use of peer-reviewed sources for all critical control and estimation algorithms complete with official documentation explaining all critical code. -1. Prioritize high-bandwidth, low-latency communication with a companion computer running ROS. +2. Robust software-in-the-loop (SIL) simulation tools for rapid testing and development of UAV code. +3. The extensive use of peer-reviewed sources for all critical control and estimation algorithms complete with official documentation explaining all critical code. +4. Prioritize high-bandwidth, low-latency communication with a companion computer running ROS. These objectives will allow researchers to more easily develop, test and field UAV code by prioritizing offboard control, good documentation and robust development tools. diff --git a/boards/airbourne/Makefile b/boards/airbourne/Makefile index c9c9105c..7e8bf89a 100644 --- a/boards/airbourne/Makefile +++ b/boards/airbourne/Makefile @@ -128,7 +128,10 @@ AIRBOURNE_SRCS = led.cpp \ uart.cpp \ M25P16.cpp \ ms4525.cpp \ - backup_sram.cpp + backup_sram.cpp \ + analog_digital_converter.cpp \ + analog_pin.cpp \ + battery_monitor.cpp \ # board-specific source files diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index 9efda7fd..2e18392f 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 9efda7fd3b6f0c86284f04ebc9b5dda73d55392d +Subproject commit 2e18392f62926d1f83cc97f2073e8867166f5313 diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index e342011f..2d67bd17 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -145,6 +145,8 @@ void AirbourneBoard::sensors_init() sonar_.init(&ext_i2c_); airspeed_.init(&ext_i2c_); // gnss_.init(&uart1_); + battery_adc_.init(battery_monitor_config.adc); + battery_monitor_.init(battery_monitor_config, &battery_adc_, 0,0); } uint16_t AirbourneBoard::num_sensor_errors() @@ -331,6 +333,36 @@ GNSSRaw AirbourneBoard::gnss_raw_read() return raw; } +bool AirbourneBoard::battery_voltage_present() const +{ + return this->battery_monitor_.has_voltage_sense(); +} + +float AirbourneBoard::battery_voltage_read() const +{ + return static_cast(this->battery_monitor_.read_voltage()); +} + +void AirbourneBoard::battery_voltage_set_multiplier(double multiplier) +{ + this->battery_monitor_.set_voltage_multiplier(multiplier); +} + +bool AirbourneBoard::battery_current_present() const +{ + return this->battery_monitor_.has_current_sense(); +} + +float AirbourneBoard::battery_current_read() const +{ + return static_cast(this->battery_monitor_.read_current()); +} + +void AirbourneBoard::battery_current_set_multiplier(double multiplier) +{ + this->battery_monitor_.set_current_multiplier(multiplier); +} + // PWM void AirbourneBoard::rc_init(rc_type_t rc_type) { @@ -432,16 +464,27 @@ void AirbourneBoard::led1_toggle() led2_.toggle(); } -//Backup memory -bool AirbourneBoard::has_backup_data() +// Backup memory +void AirbourneBoard::backup_memory_init() { - BackupData backup_data = backup_sram_read(); - return (check_backup_checksum(backup_data) && backup_data.error_code!=0); + backup_sram_init(); } -rosflight_firmware::BackupData AirbourneBoard::get_backup_data() +bool AirbourneBoard::backup_memory_read(void *dest, size_t len) { - return backup_sram_read(); + backup_sram_read(dest, len); + return true; //!< @todo backup_sram_read() has no return value } +void AirbourneBoard::backup_memory_write(const void *src, size_t len) +{ + backup_sram_write(src, len); +} + +void AirbourneBoard::backup_memory_clear(size_t len) +{ + backup_sram_clear(len); +} + + } // namespace rosflight_firmware diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 3a8fe94f..69163945 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -57,6 +57,9 @@ #include "uart.h" #include "mb1242.h" #include "backup_sram.h" +#include "analog_digital_converter.h" +#include "analog_pin.h" +#include "battery_monitor.h" // #include "ublox.h" #include "board.h" @@ -88,6 +91,8 @@ class AirbourneBoard : public Board LED led2_; LED led1_; M25P16 flash_; + AnalogDigitalConverter battery_adc_; + BatteryMonitor battery_monitor_; // UBLOX gnss_; enum SerialDevice : uint32_t @@ -163,6 +168,14 @@ class AirbourneBoard : public Board bool gnss_present() override; void gnss_update() override; + bool battery_voltage_present() const override; + float battery_voltage_read() const override; + void battery_voltage_set_multiplier(double multiplier) override; + + bool battery_current_present() const override; + float battery_current_read() const override; + void battery_current_set_multiplier(double multiplier) override; + //GNSS GNSSData gnss_read() override; bool gnss_has_new_data() override; @@ -191,9 +204,11 @@ class AirbourneBoard : public Board void led1_off() override; void led1_toggle() override; - //Backup Data - bool has_backup_data() override; - rosflight_firmware::BackupData get_backup_data() override; + // Backup Data + void backup_memory_init() override; + bool backup_memory_read(void *dest, size_t len) override; + void backup_memory_write(const void *src, size_t len) override; + void backup_memory_clear(size_t len) override; }; } // namespace rosflight_firmware diff --git a/boards/airbourne/main.cpp b/boards/airbourne/main.cpp index 6b0d102a..a56df3c6 100644 --- a/boards/airbourne/main.cpp +++ b/boards/airbourne/main.cpp @@ -36,21 +36,13 @@ #include "board.h" #include "state_manager.h" -//#pragma GCC diagnostic ignored "-Wmissing-field-initializers" //Because this was unnecessary and annoying - -uint32_t error_count_ = 0;//Used for counting resets -rosflight_firmware::ROSflight *rosflight=nullptr;//Used to access important variables in case of a hard fault -rosflight_firmware::StateManager::State get_state()//Used in case of a hard fault +rosflight_firmware::ROSflight* rosflight_ptr = nullptr; // used to access firmware in case of a hard fault +void write_backup_data(const rosflight_firmware::StateManager::BackupData::DebugInfo& debug) { - if (rosflight==nullptr) - { -#pragma GCC diagnostic push //Ignore blank fields in struct -#pragma GCC diagnostic ignored "-Wmissing-field-initializers" - rosflight_firmware::StateManager::State ret = {0}; -#pragma GCC diagnostic pop - return ret; - } - return rosflight->state_manager_.state(); + if (rosflight_ptr != nullptr) + { + rosflight_ptr->state_manager_.write_backup_data(debug); + } } extern "C" { @@ -100,29 +92,13 @@ extern "C" { pc = pulFaultStackAddress[ 6 ]; psr = pulFaultStackAddress[ 7 ]; - // avoid compiler warnings about unused variables - (void) r0; - (void) r1; - (void) r2; - (void) r3; - (void) r12; - (void) lr; - (void) pc; - (void) psr; - /* When the following line is hit, the variables contain the register values. */ - //save crash information to backup SRAM - rosflight_firmware::BackupData backup_data; - backup_data.debug_info= {r0,r1,r2,r3,r12,lr,pc,psr}; - backup_data.reset_count=error_count_+1; - backup_data.error_code=1; - backup_data.state = get_state(); - if (backup_data.state.armed) - backup_data.arm_status=rosflight_firmware::ARM_MAGIC;//magic number for extra certainty on rearm - backup_data.checksum=generate_backup_checksum(backup_data); - backup_sram_write(backup_data); + // save crash information to backup SRAM + rosflight_firmware::StateManager::BackupData::DebugInfo debug = { r0, r1, r2, r3, r12, lr, pc, psr }; + write_backup_data(debug); + // reboot NVIC_SystemReset(); } @@ -151,26 +127,25 @@ extern "C" { { while (1) {} } -} +} // extern "C" int main(void) { - rosflight_firmware::AirbourneBoard board; - rosflight_firmware::Mavlink mavlink(board); - rosflight_firmware::ROSflight firmware(board, mavlink); - rosflight = &firmware; //this allows crashes to grab some info - board.init_board(); - firmware.init(); - //Because the USB driver breaks the backup sram, the backup sram must be initalized after - backup_sram_init(); - rosflight_firmware::BackupData backup_data = backup_sram_read(); - error_count_ = backup_data.reset_count; - - while (true) - { - firmware.run(); - } - return 0; + rosflight_firmware::AirbourneBoard board; + rosflight_firmware::Mavlink mavlink(board); + rosflight_firmware::ROSflight firmware(board, mavlink); + + rosflight_ptr = &firmware; // this allows crashes to grab some info + + board.init_board(); + firmware.init(); + + while (true) + { + firmware.run(); + } + + return 0; } diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 835121f0..e405a2b6 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -307,6 +307,31 @@ uint16_t num_sensor_errors() return i2cGetErrorCounter(); } +bool BreezyBoard::battery_voltage_present() const +{ + return false; +} +float BreezyBoard::battery_voltage_read() const +{ + return 0; +} +void BreezyBoard::battery_voltage_set_multiplier(double multiplier) +{ + (void)multiplier; +} + +bool BreezyBoard::battery_current_present() const +{ + return false; +} +float BreezyBoard::battery_current_read() const +{ + return 0; +} +void BreezyBoard::battery_current_set_multiplier(double multiplier) +{ + (void)multiplier; +} // PWM void BreezyBoard::rc_init(rc_type_t rc_type) @@ -411,20 +436,6 @@ void BreezyBoard::led1_toggle() LED1_TOGGLE; } -bool BreezyBoard::has_backup_data() -{ - return false; -} - -BackupData BreezyBoard::get_backup_data() -{ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wmissing-field-initializers" - BackupData blank_data = {0}; -#pragma GCC diagnostic pop - return blank_data; -} - } #pragma GCC diagnostic pop diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index a98b64e5..ec7f0a66 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -132,6 +132,13 @@ class BreezyBoard : public Board { return; } + bool battery_voltage_present() const override; + float battery_voltage_read() const override; + void battery_voltage_set_multiplier(double multiplier) override; + + bool battery_current_present() const override; + float battery_current_read() const override; + void battery_current_set_multiplier(double multiplier) override; GNSSData gnss_read() override; bool gnss_has_new_data() override; @@ -162,9 +169,11 @@ class BreezyBoard : public Board void led1_off() override; void led1_toggle() override; - // Backup memory - bool has_backup_data() override; - BackupData get_backup_data() override; + // Backup Data + void backup_memory_init() override {} + bool backup_memory_read(void *dest, size_t len) override { (void)dest; (void)len; return false; } + void backup_memory_write(const void *src, size_t len) override { (void)src; (void)len; } + void backup_memory_clear(size_t len) override { (void)len; } }; } // namespace rosflight_firmware diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index df01b53b..a9cc4e9b 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -349,14 +349,20 @@ void Mavlink::send_version(uint8_t system_id, const char *const version) mavlink_msg_rosflight_version_pack(system_id, compid_, &msg, version); send_message(msg); } -void Mavlink::send_error_data(uint8_t system_id, const BackupData &error_data) +void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) { mavlink_message_t msg; - bool rearm = (error_data.state.armed && error_data.arm_status==rosflight_firmware::ARM_MAGIC); - mavlink_msg_rosflight_hard_error_pack(system_id,compid_, &msg, error_data.error_code, error_data.debug_info.pc, + bool rearm = (error_data.arm_flag == StateManager::BackupData::ARM_MAGIC); + mavlink_msg_rosflight_hard_error_pack(system_id,compid_, &msg, error_data.error_code, error_data.debug.pc, error_data.reset_count, rearm); send_message(msg); } +void Mavlink::send_battery_status(uint8_t system_id, float voltage, float current) +{ + mavlink_message_t msg; + mavlink_msg_rosflight_battery_status_pack(system_id, compid_, &msg, voltage, current); + send_message(msg); +} void Mavlink::send_message(const mavlink_message_t &msg) { diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index ac4283a4..3ce6eee6 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -37,6 +37,8 @@ #pragma GCC diagnostic ignored "-Wswitch-default" #pragma GCC diagnostic ignored "-Wcast-align" #pragma GCC diagnostic ignored "-Wignored-qualifiers" +#pragma GCC diagnostic ignored "-Wpragmas" +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" #include "v1.0/rosflight/mavlink.h" # pragma GCC diagnostic pop @@ -97,7 +99,8 @@ class Mavlink : public CommLinkInterface void send_version(uint8_t system_id, const char * const version) override; void send_gnss(uint8_t system_id, const GNSSData& data) override; void send_gnss_raw(uint8_t system_id, const GNSSRaw& data) override; - void send_error_data(uint8_t system_id, const BackupData& error_data) override; + void send_error_data(uint8_t system_id, const StateManager::BackupData& error_data) override; + void send_battery_status(uint8_t system_id, float voltage, float current) override; inline void set_listener(ListenerInterface * listener) override { listener_ = listener; } diff --git a/docs/developer-guide/building-flashing.md b/docs/developer-guide/building-flashing.md index d3c62e6d..ffe07a18 100644 --- a/docs/developer-guide/building-flashing.md +++ b/docs/developer-guide/building-flashing.md @@ -1,24 +1,17 @@ # Building and Flashing the Firmware -!!! Warning - Deprecation Notice: As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. +!!! warning "Deprecation Notice" + As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. -## Installing the ARM Embedded Toolchain - -To build the firmware, you will need a supported version of the ARM embedded toolchain (the compiler). If you are running Ubuntu on an ARM computer, you can simply install gcc with `apt`. Otherwise, you will need to manually install the ARM gcc compiler. - -Currently (as of June 2019), we are targeting version 6.3.1 of the "gcc-arm-none-eabi" compiler provided by the [6-2017-q2-update](https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2017q2/gcc-arm-none-eabi-6-2017-q2-update-linux.tar.bz2). -Install this version of the toolchain by downloading the archive from the ARM website and extracting to your `/opt` directory: +This guide assumes you are running Ubuntu 18.04, which is the currently supported development environment. -```bash -wget https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2017q2/gcc-arm-none-eabi-6-2017-q2-update-linux.tar.bz2 -tar -C /opt -xf gcc-arm-none-eabi-6-2017-q2-update-linux.tar.bz2 -``` +## Installing the ARM Embedded Toolchain -Add the following line to `~/.bashrc` (or your equivalent) to add the toolchain to your path: +Currently (as of March 2020) we are targeting version 7.3.1 of the ARM embedded toolchain. This toolchain can be installed from the GNU Arm Embedded Toolchain PPA: -```bash -export PATH=$PATH:/opt/gcc-arm-none-eabi-6-2017-q2-update/bin +``` bash +sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa +sudo apt install gcc-arm-embedded ``` You can test the installation and check which version is installed by running `arm-none-eabi-gcc --version`. diff --git a/docs/developer-guide/contribution-guidelines.md b/docs/developer-guide/contribution-guidelines.md index f69e0525..38096a87 100644 --- a/docs/developer-guide/contribution-guidelines.md +++ b/docs/developer-guide/contribution-guidelines.md @@ -1,7 +1,7 @@ # Contributing to the Firmware !!! Note - These documents are designed to help developers get up and running on developing new features by explaining firmware internals. Development is currently supported only on Ubuntu Linux 16.04 and 18.04. + These documents are designed to help developers get up and running on developing new features by explaining firmware internals. Development is currently supported only on Ubuntu Linux 18.04. Per our vision stated in the [introduction](../index.md), ROSflight is intended to be a streamlined, bare-bones autopilot. We welcome any bug fixes, cleanup, or other contributions which do not add complexity or detract from the readability and simplified nature of the firmware. We hope that the firmware is useful, but in an attempt to avoid "feature creep," we will be very discriminatory in merging pull requests whose purpose is to simply add features. Forking the repository in order to add features is totally acceptable and encouraged, just stay in contact with us, and recognize us as the original authors of the autopilot (per the agreement in the BSD-3 license). diff --git a/docs/developer-guide/debugging.md b/docs/developer-guide/debugging.md index b8b3a291..a8262544 100644 --- a/docs/developer-guide/debugging.md +++ b/docs/developer-guide/debugging.md @@ -1,64 +1,144 @@ # Using an In-Circuit Debugger -Debugging a Naze32 is easiest with an ST-Link V2. You can find these on Amazon and other websites. The following guide will get you up and running with QtCreator or Visual Studio Code and the in-circuit debugger. +Debugging an STM32-based board is accomplished with an ST-LINK/V2 in-circuit debugger and programmer. We have had the best luck with the official version from STMicroelectronics. These devices are reasonably priced, and are available [directly from STMicroelectronics](https://www.st.com/en/development-tools/st-link-v2.html) or from vendors such as [Digi-Key](https://www.digikey.com/product-detail/en/stmicroelectronics/ST-LINK-V2/497-10484-ND/2214535), [Mouser](https://www.mouser.com/ProductDetail/STMicroelectronics/ST-LINK-V2?qs=H4BOwPtf9MC1sDQ8j3cy4w%3D%3D), and [Newark](https://www.newark.com/stmicroelectronics/st-link-v2/icd-programmer-usb-2-0-jtag-for/dp/46T6935). -!!! warning - We have had reports of problems with cheap clones of ST-Links not connecting. +!!! note + There are also cheaper clones of the ST-LINK/V2 available. We have had fairly good luck with these, but have also run into some issues. These are a decent alternative, but we recommend the official version if you can afford it. -## Add User to Dialout Group +The following guide will show you how to get the in-circuit debugger running with either the Visual Studio Code or QtCreator IDE. Start with the steps in the [General Setup](#general-setup) section, then move on to either the [VS Code](#vs-code) or [QtCreator](#qtcreator) sections depending on your choice of IDE. -!!! tip - You can see which groups you are in by running `groups $USER` on the command line. +This guide assumes you are running Ubuntu 18.04, which is the currently supported development environment. -First, make sure you are in the `dialout` group. If you are not in the `dialout` group, run: + +## General Setup + +Follow the guide in [Building and Flashing](/developer-guide/building-flashing) to install the compiler toolchain. + +Also make sure you have configured your computer as described in the [Serial Port Configuration](../user-guide/flight-controller-setup.md#serial-port-configuration) section of the user guide. + +### Connect debugger to flight controller + +The ST-LINK/V2 connects to the microcontroller using the Serial Wire Debug (SWD) interface. You will need to connect the `GND`, `NRST`, `SWDIO`, and `SWCLK` lines of the ST-LINK/V2 to your flight controller. On many F4 boards, these lines are pinned out through a 4-position JST SH connector, although that connector is not always populated. Refer to the documentation for your specific board for details. + +The official ST-LINK/V2 also needs a target voltage reference on pin 1 or 2, which for the F4 boards is 3.3V. However, there is no externally accessible 3.3V pinout on the F4 boards. An easy solution to this is to connect pin 19 (VDD 3.3V) of the ST-LINK/V2 to pin 1 or 2 of the ST-LINK/V2 (Target VCC) to provide the voltage reference. You will also need to power the board from another source, either through the USB port or over the servo rail. Note that this connection is not required for the cheap clone versions of the ST-LINK/V2. + +## VS Code + +You can install Visual Studio Code by downloading the latest version from [their website](https://code.visualstudio.com). Follow the steps below to configure debugging with the in-circuit debugger. + +You should open the root firmware directory for editing and debugging, e.g. `code /path/to/firmware`. + +### Install OpenOCD + +OpenOCD (On-Chip Debugger) is the software that will control the debugger. Install from the `apt` repositories: ``` bash -sudo usermod -aG dialout $USER +sudo apt install openocd ``` -Log out and back in for changes to take effect. +### Install Cortex-Debug extension -## Install IDE +The embedded debugging functionality is provided by the `Cortex-Debug` extension. Install using the VS Code GUI, or from VS Code press `Ctrl+P` then type `ext install marus25.cortex-debug`. -### QtCreator +Steps for configuring this extension are described next. -For some reason, the QtCreator bundled with 16.04 is unstable. Use the most recent build of QtCreator which can be downloaded [here](https://www.qt.io/download). If you are on 18.04, you can install via apt. +### Download SVD file +A System View Description (SVD) file describes the configuration (CPU, peripherals, registers, etc.) of the microcontroller. The Cortex-Debug extension can make use of an SVD file to provide more detailed debugging information, such as the ability to inspect register values. -The following instructions are for installing Qt from the Qt provided installer: +SVD files can be downloaded from STMicroelectronics. The files for the F4 are contained in the ZIP file that can be downloaded [here](https://www.st.com/resource/en/svd/stm32f4_svd.zip), and the relevant file is `STM32F405.svd`. The files for the F1 are contained in the ZIP file that can be downloaded [here](https://www.st.com/resource/en/svd/stm32f1_svd.zip), and the relevant file is `STM32F103.svd`. Put those files in a convenient location. -This downloads a `.run` file; just make it exectuable and run as `sudo`: +### Configure build step -```bash -cd ~/Downloads -chmod +x qt-unified-linux-x64-3.0.4-online.run -sudo ./qt-unified-linux-x64-3.0.4-online.run +You can configure VS Code to run `make` for you when you press `Ctrl+Shift+B`. To do this, put the following in `.vscode/tasks.json` inside your firmware working directory: + +``` json +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "type": "shell", + "label": "make", + "command": "make", + "args": ["DEBUG=GDB"], + "group": { + "kind": "build", + "isDefault": true + } + } + ] +} ``` -If you want the icon to appear in your unity menu, create the following file as `~/.local/share/applications/qtcreator.desktop` (assuming that you installed qtcreator to the Qt folder in the installer) +Note that by default, this will only build the F4 (Revo) firmware. To build the F1 firmware, you will need to edit this to add the argument `BOARD=NAZE`. -``` -[Desktop Entry] -Exec=bash -i -c /opt/Qt/Tools/QtCreator/bin/qtcreator.sh %F -Icon=qtcreator -Type=Application -Terminal=false -Name=Qt Creator -GenericName=Integrated Development Environment -MimeType=text/x-c++src;text/x-c++hdr;text/x-xsrc;application/x-designer;application/vnd.nokia.qt.qmakeprofile;application/vnd.nokia.xml.qt.resource; -Categories=Qt;Development;IDE; -InitialPreference=9 +### Configure debugging + +To configure in-circuit debugging of F4 and F1 targets, put something like the following in `.vscode/launch.json` inside your firmware working repository: + +``` json +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "STM32F405", + "type": "cortex-debug", + "request": "launch", + "servertype": "openocd", + "cwd": "${workspaceRoot}", + "executable": "${workspaceRoot}/boards/airbourne/build/rosflight_REVO_Debug.elf", + "device": "STM32F405", + "svdFile": "/path/to/STM32F405.svd", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f4x.cfg" + ], + "runToMain": true + }, + { + "name": "STM32F103", + "type": "cortex-debug", + "request": "launch", + "servertype": "openocd", + "cwd": "${workspaceRoot}", + "executable": "${workspaceRoot}/boards/breezy/build/rosflight_NAZE_Debug.elf", + "device": "STM32F103", + "svdFile": "/path/to/STM32F103.svd", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f1x.cfg" + ], + "runToMain": true + } + ] +} ``` -### VSCode +Be sure to edit the values of `"svdFile"` to point to the respective SVD files you downloaded earlier. -You can install Visual Studio Code by downloading the latest version from **[their website](https://code.visualstudio.com)**. The debugging tools provided by VSCode have been confirmed to work on both Mac and Linux. +To start debugging, enter the debug pane in VS Code, select the desired configuration, then click the green arrow to start debugging. The shortcut key `F5` will also launch the last-selected debug configuration. -## Install openocd +More details on the configuration and use of the `Cortex-Debug` extension can be found [here](https://marcelball.ca/projects/cortex-debug/cortex-debug-launch-configurations/) and [here](https://github.com/Marus/cortex-debug). -### For QtCreator -Open OCD (On-Chip-Debugger) is the software that will control the debugger. We are going to install the version that is configured to work as a plugin for the eclipse IDE. To get this version, go to the **[releases](https://github.com/gnuarmeclipse/openocd/releases)** page of the OpenOCD github page and download the latest `.tgz` file. You can use the following commands, substituting the version you downloaded for : +## QtCreator + +Install QtCreator with apt: + +```bash +sudo apt install qtcreator +``` + +then follow the steps below to set up ARM debugging. + +### Install OpenOCD + +OpenOCD (On-Chip Debugger) is the software that will control the debugger. We are going to install the version that is configured to work as a plugin for the Eclipse IDE. To get this version, go to the [releases](https://github.com/gnuarmeclipse/openocd/releases) page of the OpenOCD github page and download the latest `.tgz` file. You can use the following commands, substituting the version you downloaded for ``: ```bash @@ -95,35 +175,9 @@ mv start_openocd_f1 /usr/local/bin mv start_openocd_f4 /usr/local/bin ``` -### For VSCode +### Install 32-bit Dependencies -For VSCode, install the openocd version currently available through your package manager: - -Ubuntu: -```bash -sudo apt install openocd -``` - -Mac: -```bash -brew install open-ocd -``` - -The `start_openocd_f4` script requires a few additional parameters to ensure proper connection to VSCode and GDB: - -```bash -#!/bin/bash -cd /opt/openocd/0.10.0-11-20190118-1134/bin -./openocd -f interface/stlink.cfg -f target/stm32f4x.cfg -c "gdb_port 50250" -c init -c "reset init" -``` - -As shown above, this script can be added to your `/usr/local/bin` if you want to be able to call it from anywhere. - -## Install ARM compiler and 32-bit Dependencies - -Follow the guide in [Building and Flashing](/developer-guide/building-flashing) to install the compiler. - -QtCreator also needs 32-bit python bindings to run GDB (skip this if using VSCode) +QtCreator needs 32-bit python bindings to run GDB. Install these with ``` bash sudo dpkg --add-architecture i386 @@ -131,11 +185,11 @@ sudo apt update sudo apt install libpython2.7:i386 ``` -## Configure QtCreator for ARM Development +### Configure QtCreator for ARM Development -Open up the new QtCreator you just installed (make sure it's the new one, and not the version you get from `apt`) +Open QtCreator and perform the following steps: -### Turn on the "Bare Metal Plugin" +#### Turn on the "Bare Metal Plugin" Help -> About Plugins -> Enable "Bare Metal" @@ -143,7 +197,7 @@ Restart QtCreator Now, we are going to configure a new "Kit" for ARM development. (This allows you to quickly switch back and forth between ARM and normal development.) -### Tell QtCreator where to find the compiler (GCC) +#### Tell QtCreator where to find the compiler (GCC) * Tools -> Options -> Build & Run -> Compilers -> Add -> GCC -> C++. * Name the new compiler, e.g. "G++ ARM" @@ -159,7 +213,7 @@ Do the same for GCC (if you are going to be doing any C-only code) ![choosing_compiler](images/choosing_compiler_screenshot.png) -### Add the Debugger (GDB) +#### Add the Debugger (GDB) * Tools -> Options -> Build & Run -> Debuggers -> Add -> GDB. * Name it something @@ -168,7 +222,7 @@ Do the same for GCC (if you are going to be doing any C-only code) ![choosing_debugger](images/choosing_debugger.png) -### Configure the STLink-V2 with OpenOCD +#### Configure the STLink-V2 with OpenOCD Go to the Bare Metal Plugin @@ -182,7 +236,7 @@ Go to the Bare Metal Plugin ![configuring_stlink](images/configuring_STLink.png) -### Build the new Development Kit +#### Build the new Development Kit * Tools -> Options -> Build & Run -> Kits -> Add * Name: ARM @@ -194,56 +248,12 @@ Go to the Bare Metal Plugin ![ARM_kit](images/ARM_kit.png) -## Configure VSCode for ARM Development - -Open the debugger launch.json file by navigating to the Debug pane (Ctrl + Shift + D) and clicking the gear at the top of the screen: - -![Open vscode debugger launch.json](images/vscode-debuggerLaunch.png) - -Add a configuration entry to the launch.json file that looks something like this (be sure to substitute the correct folder name for your version of the gcc-arm compiler): - -```json -{ - "name": "GDB-REVO", - "type": "cppdbg", - "request": "launch", - "MIMode": "gdb", - "targetArchitecture": "arm", - "miDebuggerPath": "/opt/gcc-arm-none-eabi-5_4-2016q3/bin/arm-none-eabi-gdb", - "program": "${workspaceRoot}/boards/airbourne/build/rosflight_REVO_Debug.elf", - "externalConsole": false, - "cwd": "${workspaceRoot}", - "setupCommands": [ - { "text": "file ${workspaceRoot}/boards/airbourne/build/rosflight_REVO_Debug.elf" }, - { "text": "set remotetimeout 30" }, - { "text": "target remote localhost:50250" }, - { "text": "monitor halt" }, - { "text": "monitor reset init" }, - { "text": "load" }, - { "text": "info target" } - ], -} -``` - -Note that you will need to change the `program` and first `setupCommands.text` entries if you want to run a different example or version of the firmware. - -With a board plugged in and openocd running, you should now be able to press Play on the debug screen and launch the firmware in debug mode! - -If you do not want to call `make` from the terminal for every change, you can also create a simple task in VSCode to do it for you. Open tasks.json from Command Pallette -> Tasks: Configure Task - -```json -{ - "label": "build", - "type": "shell", - "command": "make" -} -``` -## Test the Debugger +### Test the Debugger Here are the instructions for an F1 target. The instructions are very similar for an F4, just choose the correct `.elf` file. -### Turn on the Debugger +#### Turn on the Debugger Connect the debugger to your flight controller. Here is the pinout for the Flip32 and Flip32+: ![Flip32 pinout](http://www.dronetrest.com/uploads/db5290/694/14344b7ed01cb324.jpg) @@ -252,13 +262,13 @@ Plug in the debugger and start openocd (you will need sudo privileges): `sudo start_openocd_f1` -### Build the Correct Example Code +#### Build the Correct Example Code * Import Existing Project * Open the root of the firmware * Do _**not**_ add .creator files to the Git repository -### Configure the Build Environment +#### Configure the Build Environment * Go to the "Projects" tab on the left hand side * Switch to the ARM Kit we just created diff --git a/docs/user-guide/flight-controller-setup.md b/docs/user-guide/flight-controller-setup.md index 64ce767c..b227bcd9 100644 --- a/docs/user-guide/flight-controller-setup.md +++ b/docs/user-guide/flight-controller-setup.md @@ -6,8 +6,8 @@ ## Compatible Hardware -!!! Warning - Deprecation Notice: As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. +!!! warning "Deprecation Notice" + As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. As of January 2018, ROSflight is only supported on flight controllers with STM32F103 and STM32F405 processors, specifically, the Revo, Naze32, and Flip32. Both the 6-DOF and 10-DOF versions of each board are fully supported. We have had the most success with Revo boards purchased from [HobbyKing](https://hobbyking.com/en_us/openpilot-cc3d-revolution-revo-32bit-flight-controller-w-integrated-433mhz-oplink.html?___store=en_us). We have had weird issues with knock-off boards from Chinese vendors. An acro version (IMU-Only) can be found at [readytoflyquads](https://www.readytoflyquads.com/openpilot-cc3d-revolution-acro). diff --git a/docs/user-guide/hardware-setup.md b/docs/user-guide/hardware-setup.md index c43a7f37..d042cb68 100644 --- a/docs/user-guide/hardware-setup.md +++ b/docs/user-guide/hardware-setup.md @@ -39,8 +39,8 @@ Some things to keep in mind as you design or build your MAV. ROSflight is best supported on the Openpilot Revolution from [hobbyking.com](https://hobbyking.com/en_us/openpilot-cc3d-revolution-revo-32bit-flight-controller-w-integrated-433mhz-oplink.html?___store=en_us). It works on most variants of the Revo and Naze32 flight controller. Configuring a new board is relatively straight-forward, assuming that the board uses an STM32F4xx or STM32F1xx processor. -!!! Warning - Deprecation Notice: As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. +!!! warning "Deprecation Notice" + As of June 2019, plans are to deprecate support for the F1 in the near future. If you need to use an F1, you will need to retrieve an older version of the code that supports the F1. However, if there are issues, we will not be able to help you fix them. !!! warning We have seen some problems using off-brand versions of flight controllers because the accelerometers are of very poor quality, which can mess with the firmware; try to avoid those if you can. @@ -52,6 +52,7 @@ Additional Sensors you may want for your ROSflight setup include: * Sonar - MB1242 I2CXL-MaxSonar - [$40 on MaxBotix](https://www.maxbotix.com/Ultrasonic_Sensors/MB1242.htm) * GPS – u-blox NEO-M8N – [$35 from Drotek](https://drotek.com/shop/en/u-blox/883-ublox-neo-m8-gps-module.html) * Digital Airspeed Sensor – [$65 on JDrones](http://store.jdrones.com/digital_airspeed_sensor_p/senair02kit.html) +* Battery Monitor - [$10 from RCTimer](http://rctimer.com/?product-1096.html) or [DIY](https://opwiki.readthedocs.io/en/latest/user_manual/revo/voltage_current.html#basic-voltage-sensor) ### Vibration Isolation @@ -98,6 +99,15 @@ You will need a laptop which can run Ubuntu 16.04 or 18.04 with ROS to communica A joystick is used for [software-in-the-loop (SIL) simulations](gazebo_simulation.md). The joystick is not technically a required component because it is possible to control your MAV from the command line, but it makes things much easier. Our first recommendation is to use the same Taranis QX7 transmitter you use for hardware as a joystick by plugging it into the computer via USB. We also support RealFlight controllers and XBOX 360 controllers. Other joysticks are supported, but you may need to create custom axis and button mappings. +### Battery Monitor + +A battery monitor is an optional analog sensor that provides battery voltage and/or battery current information. This data can be used to prevent power loss in air or to measure system load. The sensor outputs an analog voltage proportional to the battery voltage and/or current through the battery. A battery monitor can be as simple as a voltage divider, which measures only voltage. Small PCB sensors are also available that measure both voltage and current. Ensure that the monitor output does not exceed 3.3V, as this may damage the flight controller. The battery monitor connects to the "PWR/SONAR" port on the Revo. Battery monitors are not supported on the Naze32/Flip32. + +For ROSflight to use a battery monitor, an appropriate multiplier must be set. ROSflight multiplies the analog signal from the monitor by the multiplier to get the final reading. The monitor datasheet should contain the information needed to get the multiplier. For example, the datasheet for the AttoPilot 50V/90A sensor states that it outputs 63.69 mV / V. To get the original battery voltage, the multiplier must be 1/.06369, or 15.7. The multipliers for the voltage and current are set separately, with the `BATT_VOLT_MULT` and `BATT_CURR_MULT` parameters, respectively. + +ROSflight applies a simple low-pass filter to remove noise from the voltage and current measurements. These filters are configurable via the `BATT_VOLT_ALPHA` and `BATT_CURR_ALPHA` [parameters](parameter-configuration.md). The alpha value for a given cutoff frequency \\(a\\), can be calulated with: \\( \alpha = e ^ {-.01a} \\). As battery voltages do not typically change quickly, the default of 0.995 usually suffices. + +More information on battery monitor hardware, including determinining appropriate multipliers and creating a simple DIY monitor, can be found on the [OpenPilot Wiki](https://opwiki.readthedocs.io/en/latest/user_manual/revo/voltage_current.html). ## Wiring Diagram Below is an example wiring diagram for a multirotor using an MSI Cubi as a companion computer. This diagram also includes the motor power switch, which allows for the sensors, flight controller, and companion computer to be powered on while the motors are off. This is a safer way to test sensors, code, etc. as the motors are unable to spin while the switch is off. diff --git a/docs/user-guide/parameter-configuration.md b/docs/user-guide/parameter-configuration.md index 615603dd..62d223ea 100644 --- a/docs/user-guide/parameter-configuration.md +++ b/docs/user-guide/parameter-configuration.md @@ -118,6 +118,7 @@ This is a list of all ROSflight parameters, including their types, default value | STRM_RC | Rate of raw RC input stream | int | 50 | 0 | 50 | | STRM_GNSS | Maximum rate of GNSS data streaming. Higher values allow for lower latency| int | 1000 | 0 | 1000 | | STRM_GNSS_RAW | Maximum rate of raw GNSS data streaming | int | 0 | 0 | 10 | +| STRM_BATTERY | Rate of battery status stream | int | 0 | 0 | 50 | PARAM_MAX_CMD | saturation point for PID controller output | float | 1.0 | 0 | 1.0 | | PID_ROLL_RATE_P | Roll Rate Proportional Gain | float | 0.070f | 0.0 | 1000.0 | | PID_ROLL_RATE_I | Roll Rate Integral Gain | float | 0.000f | 0.0 | 1000.0 | @@ -179,6 +180,10 @@ This is a list of all ROSflight parameters, including their types, default value | GROUND_LEVEL | Altitude of ground level (m) | float | 1387.0f | -1000 | 10000 | | DIFF_PRESS_BIAS | Differential Pressure Bias (Pa) | float | 0.0f | -10 | 10 | | RC_TYPE | Type of RC input 0 - PPM, 1 - SBUS | int | 0 | 0 | 1 | +| BATT_VOLT_MULT | Battery monitor voltage multiplier | float | 0 | 0 | inf | +| BATT_CURR_MULT | Battery monitor current multiplier | float | 0 | 0 | inf | +| BATT_VOLT_ALPHA | Batter monitor voltage filter alpha. Values closer to 1 smooth the signal more. | float | 0.995 | 0 | 1 | +| BATT_CURR_ALPHA | Battery monitor current filter alpha. Values closer to 1 smooth the signal more.| float | 0.995 | 0 | 1 | | RC_X_CHN | RC input channel mapped to x-axis commands [0 - indexed] | int | 0 | 0 | 3 | | RC_Y_CHN | RC input channel mapped to y-axis commands [0 - indexed] | int | 1 | 0 | 3 | | RC_Z_CHN | RC input channel mapped to z-axis commands [0 - indexed] | int | 3 | 0 | 3 | diff --git a/include/board.h b/include/board.h index 1b347f18..6746231a 100644 --- a/include/board.h +++ b/include/board.h @@ -42,32 +42,6 @@ namespace rosflight_firmware { -struct debug_info_t -{ - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; -}; - -struct BackupData -{ - uint32_t error_code; - debug_info_t debug_info; - uint32_t reset_count; - uint32_t arm_status; //This must equals ARM_MAGIC, or else the state manager will not rearm on reboot - //TODO add state manager info - StateManager::State state; - uint32_t checksum; //With the current implementation of the checksum, this must go last -}; - -//This magic number is used to check that the firmware was armed before it reset -const uint32_t ARM_MAGIC = 0xfa11bad; - class Board { @@ -125,6 +99,14 @@ class Board virtual bool gnss_has_new_data() = 0; virtual GNSSRaw gnss_raw_read() = 0; + virtual bool battery_voltage_present() const = 0; + virtual float battery_voltage_read() const = 0; + virtual void battery_voltage_set_multiplier(double multiplier) = 0; + + virtual bool battery_current_present() const = 0; + virtual float battery_current_read() const = 0; + virtual void battery_current_set_multiplier(double multiplier) = 0; + // RC virtual void rc_init(rc_type_t rc_type) = 0; virtual bool rc_lost() = 0; @@ -150,8 +132,10 @@ class Board virtual void led1_toggle() = 0; // Backup memory - virtual bool has_backup_data() = 0; - virtual BackupData get_backup_data() = 0; + virtual void backup_memory_init() = 0; + virtual bool backup_memory_read(void *dest, size_t len) = 0; + virtual void backup_memory_write(const void *src, size_t len) = 0; + virtual void backup_memory_clear(size_t len) = 0; }; diff --git a/include/comm_manager.h b/include/comm_manager.h index 2782dc82..f54fa196 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -60,6 +60,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis STREAM_ID_BARO, STREAM_ID_SONAR, STREAM_ID_MAG, + STREAM_ID_BATTERY_STATUS, STREAM_ID_SERVO_OUTPUT_RAW, STREAM_ID_GNSS, @@ -113,6 +114,9 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis }; LogMessageBuffer log_buffer_; + StateManager::BackupData backup_data_buffer_; + bool have_backup_data_ = false; + class Stream { public: @@ -149,10 +153,10 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_baro(void); void send_sonar(void); void send_mag(void); + void send_battery_status(void); void send_gnss(void); void send_gnss_raw(void); void send_low_priority(void); - void send_error_data(void); // Debugging Utils void send_named_value_int(const char *const name, int32_t value); @@ -169,6 +173,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis Stream(0, [this]{this->send_baro();}), Stream(0, [this]{this->send_sonar();}), Stream(0, [this]{this->send_mag();}), + Stream(0, [this]{this->send_battery_status();}), Stream(0, [this]{this->send_output_raw();}), Stream(0, [this]{this->send_gnss();}), Stream(0, [this]{this->send_gnss_raw();}), @@ -195,6 +200,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_parameter_list(); void send_named_value_float(const char *const name, float value); + + void send_backup_data(const StateManager::BackupData &backup_data); }; } // namespace rosflight_firmware diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index f1b46ddf..84fceb6a 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -39,6 +39,7 @@ #include "param.h" #include "board.h" #include "sensors.h" +#include "state_manager.h" namespace rosflight_firmware { @@ -173,7 +174,8 @@ class CommLinkInterface virtual void send_version(uint8_t system_id, const char *const version) = 0; virtual void send_gnss(uint8_t system_id, const GNSSData &data) = 0; virtual void send_gnss_raw(uint8_t system_id, const GNSSRaw &data) = 0; - virtual void send_error_data(uint8_t system_id, const BackupData &error_data) = 0; + virtual void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) = 0; + virtual void send_battery_status(uint8_t system_id,float voltage, float current) = 0; // register listener virtual void set_listener(ListenerInterface *listener) = 0; diff --git a/include/param.h b/include/param.h index 8a5995eb..433fbbd8 100644 --- a/include/param.h +++ b/include/param.h @@ -63,6 +63,7 @@ enum : uint16_t PARAM_STREAM_SONAR_RATE, PARAM_STREAM_GNSS_RATE, PARAM_STREAM_GNSS_RAW_RATE, + PARAM_STREAM_BATTERY_STATUS_RATE, PARAM_STREAM_OUTPUT_RAW_RATE, PARAM_STREAM_RC_RAW_RATE, @@ -208,6 +209,14 @@ enum : uint16_t /************************/ PARAM_OFFBOARD_TIMEOUT, + /***********************/ + /*** BATTERY MONITOR ***/ + /***********************/ + PARAM_BATTERY_VOLTAGE_MULTIPLIER, + PARAM_BATTERY_CURRENT_MULTIPLIER, + PARAM_BATTERY_VOLTAGE_ALPHA, + PARAM_BATTERY_CURRENT_ALPHA, + // keep track of size of params array PARAMS_COUNT }; diff --git a/include/sensors.h b/include/sensors.h index 91e13e1e..1fd82a43 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -162,6 +162,10 @@ class Sensors : public ParamListenerInterface bool mag_present = false; bool sonar_present = false; bool diff_pressure_present = false; + + bool battery_monitor_present = false; + float battery_voltage = 0; + float battery_current = 0; }; Sensors(ROSflight &rosflight); @@ -201,6 +205,7 @@ class Sensors : public ParamListenerInterface static const int SENSOR_CAL_CYCLES; static const float BARO_MAX_CALIBRATION_VARIANCE; static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE; + static constexpr uint32_t BATTERY_MONITOR_UPDATE_PERIOD_MS = 10; class OutlierFilter { @@ -223,6 +228,7 @@ class Sensors : public ParamListenerInterface DIFF_PRESSURE, SONAR, MAGNETOMETER, + BATTERY_MONITOR, NUM_LOW_PRIORITY_SENSORS }; @@ -246,8 +252,10 @@ class Sensors : public ParamListenerInterface void correct_baro(void); void correct_diff_pressure(void); bool update_imu(void); + void update_battery_monitor(void); void update_other_sensors(void); void look_for_disabled_sensors(void); + void update_battery_monitor_multipliers(void); uint32_t last_time_look_for_disarmed_sensors_ = 0; uint32_t last_imu_update_ms_ = 0; @@ -290,6 +298,10 @@ class Sensors : public ParamListenerInterface OutlierFilter diff_outlier_filt_; OutlierFilter sonar_outlier_filt_; + uint32_t last_battery_monitor_update_ms_ = 0; + // Battery Monitor + float battery_voltage_alpha_{0.995}; + float battery_current_alpha_{0.995}; }; } // namespace rosflight_firmware diff --git a/include/state_manager.h b/include/state_manager.h index 0fc69ca1..d6f56cce 100644 --- a/include/state_manager.h +++ b/include/state_manager.h @@ -32,7 +32,10 @@ #ifndef ROSFLIGHT_FIRMWARE_STATE_MANAGER_H #define ROSFLIGHT_FIRMWARE_STATE_MANAGER_H -#include +#include "util.h" + +#include +#include namespace rosflight_firmware { @@ -75,7 +78,62 @@ class StateManager ERROR_UNCALIBRATED_IMU = 0x0020, }; - StateManager(ROSflight &parent); + /** + * @brief Stores backup data for restoring the system state after a hard fault + * + * Also stores debugging information to be sent over serial after the connection + * is restored. + */ + struct __attribute__((packed)) BackupData + { + static constexpr uint32_t ARM_MAGIC = 0xbad2fa11; //!< magic number to ensure we only arm on startup if we really intended to + + uint16_t reset_count = 0; //!< number of hard faults since normal system startup + uint16_t error_code = 0; //!< state manager error codes + uint32_t arm_flag = 0; //!< set to ARM_MAGIC if the system was armed when the hard fault occured, 0 otherwise + + /** + * @brief Low-level debugging information for case of hard fault + * + * It is up to the board support layer to populate this data + */ + struct DebugInfo + { + uint32_t r0; //!< register 0 + uint32_t r1; //!< register 1 + uint32_t r2; //!< register 2 + uint32_t r3; //!< register 3 + uint32_t r12; //!< register 12 + uint32_t lr; //!< link register + uint32_t pc; //!< program counter + uint32_t psr; //!< program status register + } debug; + + uint32_t checksum = 0; //!< checksum for data in the struct, must be the last member + + /** + * @brief Computes checksum and prepares struct to be written to backup memory + * @pre All data fields have been set, no data will be changed between calling this function and writing to memory + * @post Checksum field is set and the struct is ready to be written to backup memory + */ + void finalize() + { + checksum = checksum_fletcher16(reinterpret_cast(this), sizeof(BackupData) - sizeof(checksum)); + } + + /** + * @brief Checks whether the checksum of a struct read from backup memory is valid + * + * @return true The checksum is valid + * @return false The checksum is invalid + */ + bool valid_checksum() + { + return checksum == checksum_fletcher16(reinterpret_cast(this), sizeof(BackupData) - sizeof(checksum)); + } + }; + + StateManager(ROSflight& parent); void init(); void run(); @@ -85,6 +143,20 @@ class StateManager void set_error(uint16_t error); void clear_error(uint16_t error); + /** + * @brief Write recovery data to backup memory in the case of a hard fault + * + * This function should only be called by the hardfault interrupt handler + * + * @pre Called from hardfault interrupt handler + * @post Recovery data has been written to backup RAM and the hardfault interrupt handler may now reset the system + * + * @param debug Low-level debugging data populated by the hardfault handler + */ + void write_backup_data(const BackupData::DebugInfo& debug); + + void check_backup_memory(); + private: ROSflight &RF_; State state_; @@ -92,6 +164,8 @@ class StateManager uint32_t next_led_blink_ms_ = 0; uint32_t next_arming_error_msg_ms_ = 0; + uint32_t hardfault_count_ = 0; + enum FsmState { FSM_STATE_INIT, diff --git a/include/util.h b/include/util.h new file mode 100644 index 00000000..40eae4e7 --- /dev/null +++ b/include/util.h @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2020, James Jackson, Daniel Koch, and Trey Henrichsen, BYU MAGICC Lab + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * 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. + * + * * Neither the name of the copyright holder 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 HOLDER 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 ROSFLIGHT_FIRMWARE_UTIL_H +#define ROSFLIGHT_FIRMWARE_UTIL_H + +#include +#include +#include + +namespace rosflight_firmware +{ + +/** + * @brief Fletcher 16-bit checksum + * + * @param src Pointer to data on which to compute the checksum + * @param len Number of bytes in the data + * @param finalize Whether to finalize the checksum; set to false for intermediate chunks of non-contiguous data + * @param start Value at which to start the checksum, set for subsequent calls on chunks of non-contiguous data + * @return uint16_t Fletcher 16-bit checksum + */ +inline uint16_t checksum_fletcher16(const uint8_t *src, size_t len, bool finalize = true, uint16_t start = 0) +{ + static constexpr size_t max_block_length = 5800; // guarantee that no overflow will occur (reduce from standard value to account for values in 'start') + + uint32_t c1 = (start & 0xFF00) >> 8; + uint32_t c2 = start & 0x00FF; + + size_t block_length; + for (; len > 0; len -= block_length) + { + block_length = len < max_block_length ? len : max_block_length; + for (size_t i = 0; i < block_length; i++) + { + c1 += *(src++); + c2 += c1; + } + + c1 %= 255; + c2 %= 255; + } + + uint16_t checksum = c1 << 8 | c2; + + if (finalize && checksum == 0) + checksum = 0xFFFF; + + return checksum; +} + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_UTIL_H diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 816b43c1..fc1fb63c 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -33,6 +33,9 @@ #include #include "rosflight.h" +#include "param.h" + +#include "comm_manager.h" namespace rosflight_firmware { @@ -95,6 +98,7 @@ void CommManager::init() set_streaming_rate(STREAM_ID_GNSS, PARAM_STREAM_GNSS_RATE); set_streaming_rate(STREAM_ID_GNSS_RAW, PARAM_STREAM_GNSS_RAW_RATE); set_streaming_rate(STREAM_ID_MAG, PARAM_STREAM_MAG_RATE); + set_streaming_rate(STREAM_ID_BATTERY_STATUS, PARAM_STREAM_BATTERY_STATUS_RATE); set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, PARAM_STREAM_OUTPUT_RAW_RATE); set_streaming_rate(STREAM_ID_RC_RAW, PARAM_STREAM_RC_RAW_RATE); @@ -144,6 +148,9 @@ void CommManager::param_change_callback(uint16_t param_id) case PARAM_STREAM_RC_RAW_RATE: set_streaming_rate(STREAM_ID_RC_RAW, param_id); break; + case PARAM_STREAM_BATTERY_STATUS_RATE: + set_streaming_rate(STREAM_ID_BATTERY_STATUS, param_id); + break; default: // do nothing break; @@ -388,14 +395,11 @@ void CommManager::heartbeat_callback(void) // to the off-board computer. connected_ = true; - static bool one_time_data_sent = false; - if (!one_time_data_sent) + // send backup data if we have it buffered + if (have_backup_data_) { - // error data - if (this->RF_.board_.has_backup_data()) - { - this->send_error_data(); - } + comm_link_.send_error_data(sysid_, backup_data_buffer_); + have_backup_data_ = false; } /// JSJ: I don't think we need this @@ -489,14 +493,14 @@ void CommManager::send_output_raw(void) void CommManager::send_rc_raw(void) { // TODO better mechanism for retreiving RC (through RC module, not PWM-specific) - uint16_t channels[8] = { static_cast(RF_.board_.rc_read(0)*1000), - static_cast(RF_.board_.rc_read(1)*1000), - static_cast(RF_.board_.rc_read(2)*1000), - static_cast(RF_.board_.rc_read(3)*1000), - static_cast(RF_.board_.rc_read(4)*1000), - static_cast(RF_.board_.rc_read(5)*1000), - static_cast(RF_.board_.rc_read(6)*1000), - static_cast(RF_.board_.rc_read(7)*1000) }; + uint16_t channels[8] = { static_cast(RF_.board_.rc_read(0)*1000 + 1000), + static_cast(RF_.board_.rc_read(1)*1000 + 1000), + static_cast(RF_.board_.rc_read(2)*1000 + 1000), + static_cast(RF_.board_.rc_read(3)*1000 + 1000), + static_cast(RF_.board_.rc_read(4)*1000 + 1000), + static_cast(RF_.board_.rc_read(5)*1000 + 1000), + static_cast(RF_.board_.rc_read(6)*1000 + 1000), + static_cast(RF_.board_.rc_read(7)*1000 + 1000) }; comm_link_.send_rc_raw(sysid_, RF_.board_.clock_millis(), channels); } @@ -539,11 +543,25 @@ void CommManager::send_mag(void) if (RF_.sensors_.data().mag_present) comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); } +void CommManager::send_battery_status(void) +{ + if(RF_.sensors_.data().battery_monitor_present) + comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, + RF_.sensors_.data().battery_current); +} -void CommManager::send_error_data(void) +void CommManager::send_backup_data(const StateManager::BackupData& backup_data) { - BackupData error_data = RF_.board_.get_backup_data(); - comm_link_.send_error_data(sysid_, error_data); + if (connected_) + { + comm_link_.send_error_data(sysid_, backup_data); + } + else + { + backup_data_buffer_ = backup_data; + have_backup_data_ = true; + } + } void CommManager::send_gnss(void) diff --git a/src/param.cpp b/src/param.cpp index d02a53bc..993fffa4 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -134,6 +134,7 @@ void Params::set_defaults(void) init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 init_param_int(PARAM_STREAM_GNSS_RAW_RATE, "STRM_GNSS_RAW", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 init_param_int(PARAM_STREAM_OUTPUT_RAW_RATE, "STRM_SERVO", 50); // Rate of raw output stream | 0 | 490 init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 @@ -275,6 +276,14 @@ void Params::set_defaults(void) /********************/ init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 + /*****************************/ + /*** BATTERY MONITOR SETUP ***/ + /*****************************/ + init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f); + init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f); + init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f); + init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f); + /************************/ /*** OFFBOARD CONTROL ***/ /************************/ diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 50eb440f..ea1c6ef4 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -88,6 +88,12 @@ void ROSflight::init() // Initialize the command muxer command_manager_.init(); + + /***************************/ + /*** Hardfault Recovery ***/ + /***************************/ + + state_manager_.check_backup_memory(); } diff --git a/src/sensors.cpp b/src/sensors.cpp index eaa5a8ab..22d56dac 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -35,6 +35,7 @@ #include #include "sensors.h" +#include "param.h" #include "rosflight.h" #include @@ -78,6 +79,8 @@ void Sensors::init() diff_outlier_filt_.init(DIFF_MAX_CHANGE_RATE, DIFF_SAMPLE_RATE, 0.0f); sonar_outlier_filt_.init(SONAR_MAX_CHANGE_RATE, SONAR_SAMPLE_RATE, 0.0f); int_start_us_ = rf_.board_.clock_micros(); + + this->update_battery_monitor_multipliers(); } void Sensors::init_imu() @@ -106,6 +109,16 @@ void Sensors::param_change_callback(uint16_t param_id) case PARAM_FC_YAW: init_imu(); break; + case PARAM_BATTERY_VOLTAGE_MULTIPLIER: + case PARAM_BATTERY_CURRENT_MULTIPLIER: + update_battery_monitor_multipliers(); + break; + case PARAM_BATTERY_VOLTAGE_ALPHA: + battery_voltage_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_ALPHA); + break; + case PARAM_BATTERY_CURRENT_ALPHA: + battery_current_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_ALPHA); + break; default: // do nothing break; @@ -209,6 +222,13 @@ void Sensors::update_other_sensors() data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range); } break; + case BATTERY_MONITOR: + if(rf_.board_.clock_millis() - last_battery_monitor_update_ms_ > BATTERY_MONITOR_UPDATE_PERIOD_MS) + { + last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); + update_battery_monitor(); + } + break; default: break; } @@ -351,6 +371,21 @@ void Sensors::get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro stamp_us = data_.imu_time; } +void Sensors::update_battery_monitor() +{ + if (rf_.board_.battery_voltage_present()) + { + data_.battery_monitor_present = true; + data_.battery_voltage = data_.battery_voltage * battery_voltage_alpha_ + + rf_.board_.battery_voltage_read() * (1-battery_voltage_alpha_); + } + if(rf_.board_.battery_current_present()) + { + data_.battery_monitor_present = true; + data_.battery_current = data_.battery_current * battery_current_alpha_ + + rf_.board_.battery_current_read() * (1-battery_current_alpha_); + } +} //====================================================================== // Calibration Functions void Sensors::calibrate_gyro() @@ -642,4 +677,12 @@ bool Sensors::OutlierFilter::update(float new_val, float *val) } } +void Sensors::update_battery_monitor_multipliers() +{ + float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); + float current_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); + this->rf_.board_.battery_voltage_set_multiplier(voltage_multiplier); + this->rf_.board_.battery_current_set_multiplier(current_multiplier); +} + } // namespace rosflight_firmware diff --git a/src/state_manager.cpp b/src/state_manager.cpp index 01993af5..58b497c6 100644 --- a/src/state_manager.cpp +++ b/src/state_manager.cpp @@ -46,19 +46,13 @@ StateManager::StateManager(ROSflight &parent) : void StateManager::init() { + RF_.board_.backup_memory_init(); + set_event(EVENT_INITIALIZED); process_errors(); // Initialize LEDs RF_.board_.led1_off(); - if (RF_.board_.has_backup_data()) - { - rosflight_firmware::BackupData error_data=RF_.board_.get_backup_data(); - this->state_=error_data.state; - //Be very sure that arming is correct - if (error_data.arm_status!=rosflight_firmware::ARM_MAGIC) - this->state_.armed=false; - } } void StateManager::run() @@ -275,6 +269,55 @@ void StateManager::set_event(StateManager::Event event) RF_.comm_manager_.update_status(); } +void StateManager::write_backup_data(const BackupData::DebugInfo& debug) +{ + BackupData data; + data.reset_count = hardfault_count_ + 1; + data.error_code = state_.error_codes; + data.arm_flag = state_.armed ? BackupData::ARM_MAGIC : 0; + data.debug = debug; + + data.finalize(); + RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); +} + +void StateManager::check_backup_memory() +{ + // reinitialize to make sure backup memory is in a good state + RF_.board_.backup_memory_init(); + + // check for hardfault recovery data in backup memory + BackupData data; + if (RF_.board_.backup_memory_read(reinterpret_cast(&data), sizeof(data))) + { + if (data.valid_checksum()) + { + hardfault_count_ = data.reset_count; + + if (data.arm_flag == BackupData::ARM_MAGIC) + { + // do emergency rearm if in a good state + if (fsm_state_ == FSM_STATE_PREFLIGHT) + { + state_.armed = true; + fsm_state_ = FSM_STATE_ARMED; + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Rearming after hardfault!!!"); + } + else + { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Failed to rearm after hardfault!!!"); + } + } + + // queue sending backup data over comm link + RF_.comm_manager_.send_backup_data(data); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Recovered from hardfault!!!"); + } + + RF_.board_.backup_memory_clear(sizeof(data)); + } +} + void StateManager::process_errors() { if (state_.error_codes) diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index 0e8e73c5..f5b2b450 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -3,6 +3,7 @@ #include "rosflight.h" #include "mavlink.h" #include "test_board.h" +#include "state_manager.h" using namespace rosflight_firmware; @@ -20,6 +21,7 @@ class StateMachineTest : public ::testing::Test void SetUp() override { + board.backup_memory_clear(); rf.init(); rf.state_manager_.clear_error(rf.state_manager_.state().error_codes); // Clear All Errors to Start rf.params_.set_param_int(PARAM_MIXER, 10); @@ -389,3 +391,114 @@ TEST_F (StateMachineTest, RegainRCAfterFailsafe) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); EXPECT_EQ(rf.state_manager_.state().failsafe, false); } +constexpr uint32_t StateManager::BackupData::ARM_MAGIC; // C++ is weird +TEST_F (StateMachineTest, NormalBoot) +{ + board.backup_memory_clear(); + rf.state_manager_.check_backup_memory(); + EXPECT_EQ(rf.state_manager_.state().armed, false); + EXPECT_EQ(rf.state_manager_.state().error, false); + EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); + EXPECT_EQ(rf.state_manager_.state().failsafe, false); +} +TEST_F(StateMachineTest, CrashRecoveryDisarmed) +{ + board.backup_memory_clear(); + StateManager::BackupData data; + data.arm_flag = 0; + data.error_code = 1; + data.reset_count = 1; + data.finalize(); + board.backup_memory_write(&data, sizeof(data)); + rf.state_manager_.check_backup_memory(); + EXPECT_EQ(rf.state_manager_.state().armed, false); + EXPECT_EQ(rf.state_manager_.state().error, false); + EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); + EXPECT_EQ(rf.state_manager_.state().failsafe, false); +} +TEST_F(StateMachineTest, CrashRecoveryArmed) +{ + board.backup_memory_clear(); + StateManager::BackupData data; + data.arm_flag = StateManager::BackupData::ARM_MAGIC; + data.error_code = 1; + data.reset_count = 1; + data.finalize(); + board.backup_memory_write(&data, sizeof(data)); + rf.state_manager_.check_backup_memory(); + EXPECT_EQ(rf.state_manager_.state().armed, true); + EXPECT_EQ(rf.state_manager_.state().error, false); + EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); + EXPECT_EQ(rf.state_manager_.state().failsafe, false); +} +TEST_F(StateMachineTest, CrashRecoveryInvalidChecksum) +{ + board.backup_memory_clear(); + StateManager::BackupData data; + data.arm_flag = StateManager::BackupData::ARM_MAGIC; + data.error_code = 1; + data.reset_count = 1; + data.finalize(); + data.checksum += 1; + board.backup_memory_write(&data, sizeof(data)); + rf.state_manager_.check_backup_memory(); + EXPECT_EQ(rf.state_manager_.state().armed, false); + EXPECT_EQ(rf.state_manager_.state().error, false); + EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); + EXPECT_EQ(rf.state_manager_.state().failsafe, false); +} +TEST_F(StateMachineTest, CrashRecoveryInvalidArmMagic) +{ + board.backup_memory_clear(); + StateManager::BackupData data; + data.arm_flag = StateManager::BackupData::ARM_MAGIC-101; + data.error_code = 1; + data.reset_count = 1; + data.finalize(); + board.backup_memory_write(&data, sizeof(data)); + rf.state_manager_.check_backup_memory(); + EXPECT_EQ(rf.state_manager_.state().armed, false); + EXPECT_EQ(rf.state_manager_.state().error, false); + EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); + EXPECT_EQ(rf.state_manager_.state().failsafe, false); +} +TEST_F(StateMachineTest, WriteBackupDataDisarmed) +{ + board.backup_memory_clear(); + const StateManager::BackupData::DebugInfo debug_info{1, 2, 3, 4, 5, 6, 7, 8}; + rf.state_manager_.write_backup_data(debug_info); + StateManager::BackupData data; + board.backup_memory_read(&data, sizeof(data)); + EXPECT_EQ(data.reset_count, 1); + EXPECT_EQ(data.arm_flag, 0); + EXPECT_TRUE(data.valid_checksum()); + EXPECT_EQ(data.debug.r0, debug_info.r0); + EXPECT_EQ(data.debug.r1, debug_info.r1); + EXPECT_EQ(data.debug.r2, debug_info.r2); + EXPECT_EQ(data.debug.r3, debug_info.r3); + EXPECT_EQ(data.debug.r12, debug_info.r12); + EXPECT_EQ(data.debug.lr, debug_info.lr); + EXPECT_EQ(data.debug.pc, debug_info.pc); + EXPECT_EQ(data.debug.psr, debug_info.psr); +} + +TEST_F(StateMachineTest, WriteBackupDataArmed) +{ + board.backup_memory_clear(); + rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); + StateManager::BackupData::DebugInfo debug_info{1, 2, 3, 4, 5, 6, 7, 8}; + rf.state_manager_.write_backup_data(debug_info); + StateManager::BackupData data; + board.backup_memory_read(&data, sizeof(data)); + EXPECT_EQ(data.reset_count, 1); + EXPECT_EQ(data.arm_flag, StateManager::BackupData::ARM_MAGIC); + EXPECT_TRUE(data.valid_checksum()); + EXPECT_EQ(data.debug.r0, debug_info.r0); + EXPECT_EQ(data.debug.r1, debug_info.r1); + EXPECT_EQ(data.debug.r2, debug_info.r2); + EXPECT_EQ(data.debug.r3, debug_info.r3); + EXPECT_EQ(data.debug.r12, debug_info.r12); + EXPECT_EQ(data.debug.lr, debug_info.lr); + EXPECT_EQ(data.debug.pc, debug_info.pc); + EXPECT_EQ(data.debug.psr, debug_info.psr); +} diff --git a/test/test_board.cpp b/test/test_board.cpp index 2dba39db..298f43ca 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -68,7 +68,10 @@ void testBoard::set_imu(float *acc, float *gyro, uint64_t time_us) // setup -void testBoard::init_board() {} +void testBoard::init_board() +{ + backup_memory_clear(); +} void testBoard::board_reset(bool bootloader) {} // clock @@ -110,6 +113,33 @@ bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint return true; } +bool testBoard::backup_memory_read(void *dest, size_t len) +{ + bool success = true; + if(len > BACKUP_MEMORY_SIZE) + { + len = BACKUP_MEMORY_SIZE; + success = false; + } + memcpy(dest, backup_memory_, len); + return success; +} + +void testBoard::backup_memory_write(const void *src, size_t len) +{ + if(len > BACKUP_MEMORY_SIZE) + len = BACKUP_MEMORY_SIZE; + memcpy(backup_memory_, src, len); +} +void testBoard::backup_memory_clear(size_t len) +{ + memset(backup_memory_, 0, len); +} +void testBoard::backup_memory_clear() +{ + backup_memory_clear(BACKUP_MEMORY_SIZE); +} + void testBoard::imu_not_responding_error() {} bool testBoard::mag_present() { return false; } @@ -128,6 +158,32 @@ bool testBoard::sonar_present() { return false; } void testBoard::sonar_update() {} float testBoard::sonar_read() { return 0; } +bool testBoard::battery_voltage_present() const +{ + return false; +} +float testBoard::battery_voltage_read() const +{ + return 0; +} +void testBoard::battery_voltage_set_multiplier(double multiplier) +{ + (void)multiplier; +} + +bool testBoard::battery_current_present() const +{ + return false; +} +float testBoard::battery_current_read() const +{ + return 0; +} +void testBoard::battery_current_set_multiplier(double multiplier) +{ + (void)multiplier; +} + //GNSS is not supported on the test board GNSSData testBoard::gnss_read() { return {}; } @@ -164,17 +220,6 @@ void testBoard::led1_on() {} void testBoard::led1_off() {} void testBoard::led1_toggle() {} -//Backup memory -bool testBoard::has_backup_data() { return false; } -BackupData testBoard::get_backup_data() -{ -#pragma GCC diagnostic push //Ignore blank fields in struct -#pragma GCC diagnostic ignored "-Wmissing-field-initializers" - BackupData blank_data = {0}; -#pragma GCC diagnostic pop - return blank_data; -} - } // namespace rosflight_firmware #pragma GCC diagnostic pop diff --git a/test/test_board.h b/test/test_board.h index f0475ffc..21a968d5 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -48,6 +48,8 @@ class testBoard : public Board float acc_[3] = {0, 0, 0}; float gyro_[3] = {0, 0, 0}; bool new_imu_ = false; + static constexpr size_t BACKUP_MEMORY_SIZE{1024}; + uint8_t backup_memory_[BACKUP_MEMORY_SIZE]; public: // setup @@ -96,6 +98,13 @@ class testBoard : public Board GNSSRaw gnss_raw_read() override; bool gnss_has_new_data() override; + bool battery_voltage_present() const override; + float battery_voltage_read() const override; + void battery_voltage_set_multiplier(double multiplier) override; + + bool battery_current_present() const override; + float battery_current_read() const override; + void battery_current_set_multiplier(double multiplier) override; // RC void rc_init(rc_type_t rc_type) override; @@ -122,9 +131,11 @@ class testBoard : public Board void led1_toggle() override; //Backup memory - bool has_backup_data() override; - BackupData get_backup_data() override; - + void backup_memory_init() override {} + bool backup_memory_read(void *dest, size_t len) override; + void backup_memory_write(const void *src, size_t len) override; + void backup_memory_clear(size_t len) override; + void backup_memory_clear(); // Not an override void set_imu(float *acc, float *gyro, uint64_t time_us); void set_rc(uint16_t *values);