From 4573635c5c46d4c2b6c17be85dad1359967363d3 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:09:32 -0700 Subject: [PATCH 01/19] moved git version error to param header --- include/param.h | 15 +++++++++++++++ src/param.cpp | 15 --------------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/include/param.h b/include/param.h index 61b2235f..03d42067 100644 --- a/include/param.h +++ b/include/param.h @@ -32,6 +32,21 @@ #ifndef ROSFLIGHT_FIRMWARE_PARAM_H #define ROSFLIGHT_FIRMWARE_PARAM_H +#ifndef GIT_VERSION_HASH +#define GIT_VERSION_HASH 0x00 +#pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!" +#endif +#ifndef GIT_VERSION_STRING +#define GIT_VERSION_STRING "empty" +#pragma message "GIT_VERSION_STRING Undefined, setting to \"empty\"!" +#endif + +// Uncomment to view contents of GIT_VERSION_HASH and GIT_VERSION STRING +// #define STRINGIFY(s) XSTRINGIFY(s) +// #define XSTRINGIFY(s) #s +// #pragma message( "GIT_VERSION_HASH: " STRINGIFY(GIT_VERSION_HASH)) +// #pragma message( "GIT_VERSION_STRING: " GIT_VERSION_STRING) + #include "interface/param_listener.h" #include diff --git a/src/param.cpp b/src/param.cpp index 659e9f6a..9d46608b 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -39,21 +39,6 @@ #include #include -#ifndef GIT_VERSION_HASH -#define GIT_VERSION_HASH 0x00 -#pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!" -#endif -#ifndef GIT_VERSION_STRING -#define GIT_VERSION_STRING "empty" -#pragma message "GIT_VERSION_STRING Undefined, setting to \"empty\"!" -#endif - -// Uncomment to view contents of GIT_VERSION_HASH and GIT_VERSION STRING -// #define STRINGIFY(s) XSTRINGIFY(s) -// #define XSTRINGIFY(s) #s -// #pragma message( "GIT_VERSION_HASH: " STRINGIFY(GIT_VERSION_HASH)) -// #pragma message( "GIT_VERSION_STRING: " GIT_VERSION_STRING) - namespace rosflight_firmware { Params::Params(ROSflight & _rf) From e3e40b842fae471ad75e619846dcf33257e73fd4 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:12:34 -0700 Subject: [PATCH 02/19] removed existing firmware submodules --- .gitmodules | 6 - boards/airbourne/Makefile | 221 --------------- boards/airbourne/airbourne | 1 - boards/airbourne/airbourne_board.cpp | 390 --------------------------- boards/airbourne/airbourne_board.h | 212 --------------- boards/airbourne/main.cpp | 142 ---------- boards/breezy/Makefile | 157 ----------- boards/breezy/breezy_board.cpp | 310 --------------------- boards/breezy/breezy_board.h | 180 ------------- boards/breezy/breezystm32 | 1 - boards/breezy/flash.c | 86 ------ boards/breezy/flash.h | 79 ------ boards/breezy/main.cpp | 48 ---- 13 files changed, 1833 deletions(-) delete mode 100644 boards/airbourne/Makefile delete mode 160000 boards/airbourne/airbourne delete mode 100644 boards/airbourne/airbourne_board.cpp delete mode 100644 boards/airbourne/airbourne_board.h delete mode 100644 boards/airbourne/main.cpp delete mode 100644 boards/breezy/Makefile delete mode 100644 boards/breezy/breezy_board.cpp delete mode 100644 boards/breezy/breezy_board.h delete mode 160000 boards/breezy/breezystm32 delete mode 100644 boards/breezy/flash.c delete mode 100644 boards/breezy/flash.h delete mode 100644 boards/breezy/main.cpp diff --git a/.gitmodules b/.gitmodules index bec5d4a9..9ee88d00 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,3 @@ [submodule "comms/mavlink/v1.0"] path = comms/mavlink/v1.0 url = https://github.com/rosflight/mavlink_c_library.git -[submodule "boards/breezy/breezystm32"] - path = boards/breezy/breezystm32 - url = https://github.com/rosflight/BreezySTM32.git -[submodule "boards/airbourne/airbourne"] - path = boards/airbourne/airbourne - url = https://github.com/rosflight/airbourne_f4.git diff --git a/boards/airbourne/Makefile b/boards/airbourne/Makefile deleted file mode 100644 index 596161fe..00000000 --- a/boards/airbourne/Makefile +++ /dev/null @@ -1,221 +0,0 @@ -############################################################################### -# Generic Makefile Template for C/C++ for use with STM32 Microcontrollers -# -# Copyright (c) 2016 - James Jackson -# 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 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 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. - -TARGET = rosflight - -BOARD ?= REVO - -DEBUG ?= GDB - -SERIAL_DEVICE ?= /dev/ttyACM0 - - - -################################# -# Working directories -################################# -ROSFLIGHT_ROOT = ../.. -AIRBOURNE_ROOT = airbourne -AIRBOURNE_SRC_DIR = $(AIRBOURNE_ROOT)/src -CMSIS_DIR = $(AIRBOURNE_ROOT)/lib/CMSIS -STDPERIPH_DIR = $(AIRBOURNE_ROOT)/lib/STM32F4xx_StdPeriph_Driver -USBCORE_DIR = $(AIRBOURNE_ROOT)/lib/STM32_USB_Device_Library/Core -USBOTG_DIR = $(AIRBOURNE_ROOT)/lib/STM32_USB_OTG_Driver -USBCDC_DIR = $(AIRBOURNE_ROOT)/lib/STM32_USB_Device_Library/Class/cdc -VCP_DIR = $(AIRBOURNE_ROOT)/lib/vcp -STARTUP_DIR = $(AIRBOURNE_ROOT)/lib/startup -PRINTF_DIR = $(AIRBOURNE_ROOT)/lib/printf -BIN_DIR = $(BOARD_DIR)/build -BOARD_DIR = $(ROSFLIGHT_ROOT)/boards/airbourne - -################################# -# ROSflight Common Build -################################# -include $(ROSFLIGHT_ROOT)/scripts/rosflight.mk - - -################################# -# Source Files -################################# -VPATH := $(VPATH):$(STARTUP_DIR) -LDSCRIPT = $(STARTUP_DIR)/stm32f405.ld -ASOURCES = stm32f405.s - -# Search path and source files for the CMSIS sources -VPATH := $(VPATH):$(CMSIS_DIR)/CM4/CoreSupport -VPATH := $(VPATH):$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM4/CoreSupport/*.c)) \ - $(notdir $(wildcard $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx/*.c)) - -# Search path and source files for the ST stdperiph library and exclude files we don't need -VPATH := $(VPATH):$(STDPERIPH_DIR)/src -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) -EXCLUDES = stm32f4xx_crc.c \ - stm32f4xx_can.c \ - stm32f4xx_fmc.c \ - stm32f4xx_fsmc.c \ - stm32f4xx_sai.c \ - stm32f4xx_cec.c \ - stm32f4xx_dsi.c \ - stm32f4xx_flash_ramfunc.c \ - stm32f4xx_fmpi2c.c \ - stm32f4xx_lptim.c \ - stm32f4xx_qspi.c -STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) - - -# Search path and source files for the USB libraries and ignore files we don't need -VPATH := $(VPATH):$(USBCORE_DIR)/src:$(USBOTG_DIR)/src:$(USBCDC_DIR)/src -USBCORE_SRC = $(notdir $(wildcard $(USBCORE_DIR)/src/*.c)) -USBOTG_SRC = $(notdir $(wildcard $(USBOTG_DIR)/src/*.c)) -USBCDC_SRC = $(notdir $(wildcard $(USBCDC_DIR)/src/*.c)) -EXCLUDES = usb_bsp_template.c \ - usb_conf_template.c \ - usb_hcd_int.c \ - usb_hcd.c \ - usb_otg.c \ - usbd_cdc_if_template.c \ - usbd_cdc_core_loopback.c -USBCDC_SRC := $(filter-out ${EXCLUDES}, $(USBCDC_SRC)) -USBOTG_SRC := $(filter-out ${EXCLUDES}, $(USBOTG_SRC)) - -# Add VCP source files -VPATH := $(VPATH):$(VCP_DIR) -VCP_SRC = $(notdir $(wildcard $(VCP_DIR)/*.c)) - -# Make a list of source files and includes -VPATH := $(VPATH):$(AIRBOURNE_SRC_DIR) -AIRBOURNE_SRCS = led.cpp \ - gpio.cpp \ - spi.cpp \ - vcp.cpp \ - led.cpp \ - mpu6000.cpp \ - advanced.cpp \ - i2c.cpp \ - pwm.cpp \ - mb1242.cpp \ - eeprom.cpp \ - hmc5883l.cpp \ - ms5611.cpp \ - rc_ppm.cpp \ - rc_sbus.cpp \ - uart.cpp \ - M25P16.cpp \ - ms4525.cpp \ - backup_sram.cpp \ - analog_digital_converter.cpp \ - analog_pin.cpp \ - battery_monitor.cpp \ - ublox.cpp - - -# board-specific source files -VPATH := $(VPATH):$(BOARD_DIR) -BOARD_CXX_SRC = airbourne_board.cpp \ - main.cpp - -# Make a list of source files and includes -VPATH := $(VPATH):$(SRC_DIR) -CSOURCES = $(CMSIS_SRC) \ - $(STDPERIPH_SRC) \ - $(USBCORE_SRC) \ - $(USBOTG_SRC) \ - $(USBCDC_SRC) \ - $(PROCESSOR_SRC) \ - $(VCP_SRC) \ - system.c \ - system_stm32f4xx.c - -CXXSOURCES += $(AIRBOURNE_SRCS) \ - $(BOARD_CXX_SRC) - -INCLUDE_DIRS += $(AIRBOURNE_SRC_DIR) \ - $(AIRBOURNE_ROOT)/include \ - $(STDPERIPH_DIR)/inc \ - $(USBOTG_DIR)/inc \ - $(USBCORE_DIR)/inc \ - $(USBCDC_DIR)/inc \ - $(CMSIS_DIR)/CM4/CoreSupport \ - $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx \ - $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F4xx/Include \ - $(PRINTF_DIR) \ - $(VCP_DIR) - -################################# -# Flags -################################# - -MCFLAGS= -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion -DEFS+=-DSTM32F40_41xxx -D__CORTEX_M4 -D__FPU_PRESENT -DWORDS_STACK_SIZE=200 -DUSE_STDPERIPH_DRIVER -D__FPU_USED -DHSE_VALUE=8000000 -DUSE_USB_OTG_FS -CFLAGS += $(MCFLAGS) $(OPTIMIZE) $(addprefix -I,$(INCLUDE_DIRS)) -CXXFLAGS += $(MCFLAGS) $(OPTIMIZE) $(addprefix -I,$(INCLUDE_DIRS)) -LDFLAGS =-T $(LDSCRIPT) $(MCFLAGS) -lm -lc --specs=nano.specs --specs=rdimon.specs $(ARCH_FLAGS) $(LTO_FLAGS) $(DEBUG_FLAGS) -static -Wl,-gc-sections -Wl,-Map=main.map - -$(info VPATH = $(VPATH)) -$(info STARTUP_DIR = $(STARTUP_DIR)) - -################################# -# Build -################################# -$(TARGET_BIN): $(TARGET_HEX) - $(CP) -I ihex -O binary $< $@ - -$(TARGET_HEX): $(TARGET_ELF) - $(CP) -O ihex --set-start 0x8000000 $< $@ - -$(TARGET_ELF): $(OBJECTS) - $(CXX) -o $@ $^ $(LDFLAGS) - $(SIZE) $(TARGET_ELF) - -$(BIN_DIR)/$(TARGET)/%.o: %.cpp - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CXX) -c -o $@ $(CXXFLAGS) $< - -$(BIN_DIR)/$(TARGET)/%.o: %.c - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(CFLAGS) $< - -$(BIN_DIR)/$(TARGET)/%.o: %.s - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(CFLAGS) $< - - -################################# -# Recipes -################################# -.PHONY: all flash clean - -clean: - rm -rf $(OBJECTS) $(BIN_DIR) - -flash: $(TARGET_BIN) - dfu-util -a 0 -s 0x08000000 -D $(TARGET_BIN) -R diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne deleted file mode 160000 index 1009867d..00000000 --- a/boards/airbourne/airbourne +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 1009867d7e6d9b5730b4d58d238a276782769f8c diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp deleted file mode 100644 index 48bc19e9..00000000 --- a/boards/airbourne/airbourne_board.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ - -#include "airbourne_board.h" - -namespace rosflight_firmware -{ -AirbourneBoard::AirbourneBoard() {} - -void AirbourneBoard::init_board() -{ - systemInit(); - led2_.init(LED2_GPIO, LED2_PIN); - led1_.init(LED1_GPIO, LED1_PIN); - - int_i2c_.init(&i2c_config[BARO_I2C]); - ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); - spi1_.init(&spi_config[MPU6000_SPI]); - spi3_.init(&spi_config[FLASH_SPI]); - uart1_.init(&uart_config[UART1], 115200, UART::MODE_8N1); - uart3_.init(&uart_config[UART3], 115200, UART::MODE_8N1); - - backup_sram_init(); - - current_serial_ = &vcp_; // uncomment this to switch to VCP as the main output -} - -void AirbourneBoard::board_reset(bool bootloader) -{ - (void) bootloader; - NVIC_SystemReset(); -} - -// clock -uint32_t AirbourneBoard::clock_millis() { return millis(); } - -uint64_t AirbourneBoard::clock_micros() { return micros(); } - -void AirbourneBoard::clock_delay(uint32_t milliseconds) { delay(milliseconds); } - -// serial -void AirbourneBoard::serial_init(uint32_t baud_rate, uint32_t dev) -{ - vcp_.init(); - switch (dev) { - case SERIAL_DEVICE_UART3: - uart3_.init(&uart_config[UART3], baud_rate); - current_serial_ = &uart3_; - secondary_serial_device_ = SERIAL_DEVICE_UART3; - break; - default: - current_serial_ = &vcp_; - secondary_serial_device_ = SERIAL_DEVICE_VCP; - } -} - -void AirbourneBoard::serial_write(const uint8_t * src, size_t len) -{ - current_serial_->write(src, len); -} - -uint16_t AirbourneBoard::serial_bytes_available() -{ - if (vcp_.connected() || secondary_serial_device_ == SERIAL_DEVICE_VCP) { - current_serial_ = &vcp_; - } else { - switch (secondary_serial_device_) { - case SERIAL_DEVICE_UART3: - current_serial_ = &uart3_; - break; - default: - // no secondary serial device - break; - } - } - - return current_serial_->rx_bytes_waiting(); -} - -uint8_t AirbourneBoard::serial_read() { return current_serial_->read_byte(); } - -void AirbourneBoard::serial_flush() { current_serial_->flush(); } - -// sensors -void AirbourneBoard::sensors_init() -{ - while (millis() < 50) {} // wait for sensors to boot up - imu_.init(&spi1_); - - baro_.init(&int_i2c_); - mag_.init(&int_i2c_); - 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() { return ext_i2c_.num_errors(); } - -bool AirbourneBoard::new_imu_data() { return imu_.new_data(); } - -bool AirbourneBoard::imu_read(float accel[3], float * temperature, float gyro[3], - uint64_t * time_us) -{ - float read_accel[3], read_gyro[3]; - imu_.read(read_accel, read_gyro, temperature, time_us); - - accel[0] = -read_accel[1]; - accel[1] = -read_accel[0]; - accel[2] = -read_accel[2]; - - gyro[0] = -read_gyro[1]; - gyro[1] = -read_gyro[0]; - gyro[2] = -read_gyro[2]; - - return true; -} - -void AirbourneBoard::imu_not_responding_error() { sensors_init(); } - -bool AirbourneBoard::mag_present() -{ - mag_.update(); - return mag_.present(); -} - -void AirbourneBoard::mag_update() { mag_.update(); } - -void AirbourneBoard::mag_read(float mag[3]) -{ - mag_.update(); - mag_.read(mag); -} -bool AirbourneBoard::baro_present() -{ - baro_.update(); - return baro_.present(); -} - -void AirbourneBoard::baro_update() { baro_.update(); } - -void AirbourneBoard::baro_read(float * pressure, float * temperature) -{ - baro_.update(); - baro_.read(pressure, temperature); -} - -bool AirbourneBoard::diff_pressure_present() { return airspeed_.present(); } - -void AirbourneBoard::diff_pressure_update() { airspeed_.update(); } - -void AirbourneBoard::diff_pressure_read(float * diff_pressure, float * temperature) -{ - (void) diff_pressure; - (void) temperature; - airspeed_.update(); - airspeed_.read(diff_pressure, temperature); -} - -bool AirbourneBoard::sonar_present() { return sonar_.present(); } - -void AirbourneBoard::sonar_update() { sonar_.update(); } - -float AirbourneBoard::sonar_read() { return sonar_.read(); } - -bool AirbourneBoard::gnss_present() -{ - gnss_.check_connection_status(); - return gnss_.present(); -} -void AirbourneBoard::gnss_update() {} -bool AirbourneBoard::gnss_has_new_data() { return this->gnss_.new_data(); } -// This method translates the UBLOX driver interface into the ROSFlight interface -// If not gnss_has_new_data(), then this may return 0's for ECEF position data, -// ECEF velocity data, or both -GNSSData AirbourneBoard::gnss_read() -{ - UBLOX::GNSSPVT gnss_pvt = gnss_.read(); - UBLOX::GNSSPosECEF pos_ecef = gnss_.read_pos_ecef(); - UBLOX::GNSSVelECEF vel_ecef = gnss_.read_vel_ecef(); - uint64_t timestamp = gnss_.get_last_pvt_timestamp(); - GNSSData gnss = {}; - gnss.time_of_week = gnss_pvt.time_of_week; - bool has_fix = (gnss_pvt.fix_type == UBLOX::FIX_TYPE_3D); - gnss.fix_type = has_fix ? GNSS_FIX_TYPE_FIX : GNSS_FIX_TYPE_NO_FIX; - gnss.time = gnss_pvt.time; - gnss.nanos = gnss_pvt.nanos; - gnss.lat = gnss_pvt.lat; - gnss.lon = gnss_pvt.lon; - gnss.height = gnss_pvt.height; - gnss.vel_n = gnss_pvt.vel_n; - gnss.vel_e = gnss_pvt.vel_e; - gnss.vel_d = gnss_pvt.vel_d; - gnss.h_acc = gnss_pvt.h_acc; - gnss.v_acc = gnss_pvt.v_acc; - // Does not include ECEF position data if the timestamp doesn't match - // See UBLOX::new_data() for reasoning - if (gnss.time_of_week == pos_ecef.time_of_week) { - gnss.ecef.x = pos_ecef.x; - gnss.ecef.y = pos_ecef.y; - gnss.ecef.z = pos_ecef.z; - gnss.ecef.p_acc = pos_ecef.p_acc; - } - // Does not include ECEF position data if the timestamp doesn't match - // See UBLOX::new_data() for reasoning - if (gnss.time_of_week == vel_ecef.time_of_week) { - gnss.ecef.vx = vel_ecef.vx; - gnss.ecef.vy = vel_ecef.vy; - gnss.ecef.vz = vel_ecef.vz; - gnss.ecef.s_acc = vel_ecef.s_acc; - } - gnss.rosflight_timestamp = timestamp; - return gnss; -} -GNSSFull AirbourneBoard::gnss_full_read() -{ - UBLOX::NAV_PVT_t pvt = gnss_.read_full(); - GNSSFull full = {}; - full.time_of_week = pvt.iTOW; - full.year = pvt.time.year; - full.month = pvt.time.month; - full.day = pvt.time.day; - full.hour = pvt.time.hour; - full.min = pvt.time.min; - full.sec = pvt.time.sec; - full.valid = pvt.time.valid; - full.t_acc = pvt.time.tAcc; - full.nano = pvt.time.nano; - full.fix_type = pvt.fixType; - full.num_sat = pvt.numSV; - full.lon = pvt.lon; - full.lat = pvt.lat; - full.height = pvt.height; - full.height_msl = pvt.hMSL; - full.h_acc = pvt.hAcc; - full.v_acc = pvt.vAcc; - full.vel_n = pvt.velN; - full.vel_e = pvt.velE; - full.vel_d = pvt.velD; - full.g_speed = pvt.gSpeed; - full.head_mot = pvt.headMot; - full.s_acc = pvt.sAcc; - full.head_acc = pvt.headAcc; - full.p_dop = pvt.pDOP; - full.rosflight_timestamp = gnss_.get_last_pvt_timestamp(); - return full; -} - -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) -{ - switch (rc_type) { - case RC_TYPE_SBUS: - uart1_.init(&uart_config[UART1], 100000, UART::MODE_8E2); - inv_pin_.init(SBUS_INV_GPIO, SBUS_INV_PIN, GPIO::OUTPUT); - rc_sbus_.init(&inv_pin_, &uart1_); - rc_ = &rc_sbus_; - break; - case RC_TYPE_PPM: - default: - rc_ppm_.init(&pwm_config[RC_PPM_PIN]); - rc_ = &rc_ppm_; - break; - } -} - -float AirbourneBoard::rc_read(uint8_t channel) { return rc_->read(channel); } - -void AirbourneBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) -{ - for (int i = 0; i < PWM_NUM_OUTPUTS; i++) { - esc_out_[i].init(&pwm_config[i], refresh_rate, 2000, 1000); - esc_out_[i].writeUs(idle_pwm); - } -} - -void AirbourneBoard::pwm_disable() -{ - for (int i = 0; i < PWM_NUM_OUTPUTS; i++) { esc_out_[i].disable(); } -} - -void AirbourneBoard::pwm_write(uint8_t channel, float value) -{ - if (channel < PWM_NUM_OUTPUTS) { esc_out_[channel].write(value); } -} - -bool AirbourneBoard::rc_lost() { return rc_->lost(); } - -// non-volatile memory -void AirbourneBoard::memory_init() { return flash_.init(&spi3_); } - -bool AirbourneBoard::memory_read(void * data, size_t len) -{ - return flash_.read_config(reinterpret_cast(data), len); -} - -bool AirbourneBoard::memory_write(const void * data, size_t len) -{ - return flash_.write_config(reinterpret_cast(data), len); -} - -// LED -void AirbourneBoard::led0_on() { led1_.on(); } - -void AirbourneBoard::led0_off() { led1_.off(); } - -void AirbourneBoard::led0_toggle() { led1_.toggle(); } - -void AirbourneBoard::led1_on() { led2_.on(); } - -void AirbourneBoard::led1_off() { led2_.off(); } - -void AirbourneBoard::led1_toggle() { led2_.toggle(); } - -// Backup memory -void AirbourneBoard::backup_memory_init() { backup_sram_init(); } - -bool AirbourneBoard::backup_memory_read(void * dest, size_t len) -{ - 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 deleted file mode 100644 index 6cb15be5..00000000 --- a/boards/airbourne/airbourne_board.h +++ /dev/null @@ -1,212 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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_AIRBOURNE_BOARD_H -#define ROSFLIGHT_FIRMWARE_AIRBOURNE_BOARD_H - -#include "M25P16.h" -#include "analog_digital_converter.h" -#include "analog_pin.h" -#include "backup_sram.h" -#include "battery_monitor.h" -#include "board.h" -#include "hmc5883l.h" -#include "i2c.h" -#include "led.h" -#include "mb1242.h" -#include "mpu6000.h" -#include "ms4525.h" -#include "ms5611.h" -#include "pwm.h" -#include "rc_base.h" -#include "rc_ppm.h" -#include "rc_sbus.h" -#include "serial.h" -#include "spi.h" -#include "system.h" -#include "uart.h" -#include "ublox.h" -#include "vcp.h" - -#include - -#include -#include -#include - -namespace rosflight_firmware -{ -class AirbourneBoard : public Board -{ -private: - VCP vcp_; - UART uart1_; - UART uart3_; - Serial * current_serial_; // A pointer to the serial stream currently in use. - I2C int_i2c_; - I2C ext_i2c_; - SPI spi1_; - SPI spi3_; - MPU6000 imu_; - HMC5883L mag_; - MS5611 baro_; - MS4525 airspeed_; - RC_PPM rc_ppm_; - I2CSonar sonar_; - RC_SBUS rc_sbus_; - GPIO inv_pin_; - PWM_OUT esc_out_[PWM_NUM_OUTPUTS]; - LED led2_; - LED led1_; - M25P16 flash_; - AnalogDigitalConverter battery_adc_; - BatteryMonitor battery_monitor_; - UBLOX gnss_; - - enum SerialDevice : uint32_t - { - SERIAL_DEVICE_VCP = 0, - SERIAL_DEVICE_UART3 = 3 - }; - SerialDevice secondary_serial_device_ = SERIAL_DEVICE_VCP; - - RC_BASE * rc_ = nullptr; - - std::function imu_callback_; - - int _board_revision = 2; - - float _accel_scale = 1.0; - float _gyro_scale = 1.0; - - enum - { - SONAR_NONE, - SONAR_I2C, - SONAR_PWM - }; - uint8_t sonar_type = SONAR_NONE; - - bool new_imu_data_; - uint64_t imu_time_us_; - -public: - AirbourneBoard(); - - // setup - void init_board() override; - void board_reset(bool bootloader) override; - - // clock - uint32_t clock_millis() override; - uint64_t clock_micros() override; - void clock_delay(uint32_t milliseconds) override; - - // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t * src, size_t len) override; - uint16_t serial_bytes_available() override; - uint8_t serial_read() override; - void serial_flush() override; - - // sensors - void sensors_init() override; - uint16_t num_sensor_errors() override; - - bool new_imu_data() override; - bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) override; - void imu_not_responding_error() override; - - bool mag_present() override; - void mag_update() override; - void mag_read(float mag[3]) override; - - bool baro_present() override; - void baro_update() override; - void baro_read(float * pressure, float * temperature) override; - - bool diff_pressure_present() override; - void diff_pressure_update() override; - void diff_pressure_read(float * diff_pressure, float * temperature) override; - - bool sonar_present() override; - void sonar_update() override; - float sonar_read() override; - - 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; - GNSSFull gnss_full_read() override; - // RC - void rc_init(rc_type_t rc_type) override; - bool rc_lost() override; - float rc_read(uint8_t channel) override; - - // PWM - void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; - void pwm_disable() override; - void pwm_write(uint8_t channel, float value) override; - - // non-volatile memory - void memory_init() override; - bool memory_read(void * dest, size_t len) override; - bool memory_write(const void * src, size_t len) override; - - // LEDs - void led0_on() override; - void led0_off() override; - void led0_toggle() override; - - void led1_on() override; - void led1_off() override; - void led1_toggle() 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 - -#endif // ROSFLIGHT_FIRMWARE_AIRBOURNE_BOARD_H diff --git a/boards/airbourne/main.cpp b/boards/airbourne/main.cpp deleted file mode 100644 index 0e08b821..00000000 --- a/boards/airbourne/main.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ - -#include "airbourne_board.h" -#include "backup_sram.h" -#include "board.h" -#include "mavlink.h" -#include "state_manager.h" - -#include "rosflight.h" - -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_ptr != nullptr) { rosflight_ptr->state_manager_.write_backup_data(debug); } -} - -extern "C" { -/* The prototype shows it is a naked function - in effect this is just an - assembly function. */ -void HardFault_Handler(void) __attribute__((naked)); - -/* The fault handler implementation calls a function called - prvGetRegistersFromStack(). */ -void HardFault_Handler(void) -{ - __asm volatile(" tst lr, #4 \n" - " ite eq \n" - " mrseq r0, msp \n" - " mrsne r0, psp \n" - " ldr r1, [r0, #24] \n" - " ldr r2, handler2_address_const \n" - " bx r2 \n" - " handler2_address_const: .word prvGetRegistersFromStack \n"); -} - -void prvGetRegistersFromStack(uint32_t * pulFaultStackAddress) -{ - /* These are volatile to try and prevent the compiler/linker optimising them - away as the variables never actually get used. If the debugger won't show the - values of the variables, make them global my moving their declaration outside - of this function. */ - volatile uint32_t r0; - volatile uint32_t r1; - volatile uint32_t r2; - volatile uint32_t r3; - volatile uint32_t r12; - volatile uint32_t lr; /* Link register. */ - volatile uint32_t pc; /* Program counter. */ - volatile uint32_t psr; /* Program status register. */ - - r0 = pulFaultStackAddress[0]; - r1 = pulFaultStackAddress[1]; - r2 = pulFaultStackAddress[2]; - r3 = pulFaultStackAddress[3]; - - r12 = pulFaultStackAddress[4]; - lr = pulFaultStackAddress[5]; - pc = pulFaultStackAddress[6]; - psr = pulFaultStackAddress[7]; - - /* When the following line is hit, the variables contain the register values. */ - - // 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(); -} - -void NMI_Handler() -{ - while (1) {} -} - -void MemManage_Handler() -{ - while (1) {} -} - -void BusFault_Handler() -{ - while (1) {} -} - -void UsageFault_Handler() -{ - while (1) {} -} - -void WWDG_IRQHandler() -{ - while (1) {} -} -} // extern "C" - -int main(void) -{ - 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/Makefile b/boards/breezy/Makefile deleted file mode 100644 index de797f03..00000000 --- a/boards/breezy/Makefile +++ /dev/null @@ -1,157 +0,0 @@ -############################################################################### -# Generic Makefile Template for C/C++ for use with STM32 Microcontrollers -# -# Copyright (c) 2016 - James Jackson -# 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 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 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. - -TARGET ?= rosflight - -BOARD ?= NAZE - -DEBUG ?= GDB - -SERIAL_DEVICE ?= /dev/ttyUSB0 - - -################################# -# Working directories -################################# -ROSFLIGHT_ROOT = ../.. -BOARD_DIR = $(ROSFLIGHT_ROOT)/boards/breezy -BREEZY_DIR = breezystm32 -CMSIS_DIR = $(BREEZY_DIR)/lib/CMSIS -STDPERIPH_DIR = $(BREEZY_DIR)/lib/STM32F10x_StdPeriph_Driver -BIN_DIR = $(BOARD_DIR)/build - - -################################# -# ROSflight Common Build -################################# -include $(ROSFLIGHT_ROOT)/scripts/rosflight.mk - - -################################# -# Source Files -################################# -VPATH := $(VPATH):$(BREEZY_DIR) -LDSCRIPT = $(BREEZY_DIR)/stm32_flash.ld -ASOURCES = startup_stm32f10x_md_gcc.S - -# Search path and source files for the CMSIS sources -VPATH := $(VPATH):$(CMSIS_DIR)/CM3/CoreSupport:$(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x -CMSIS_SRC = $(notdir $(wildcard $(CMSIS_DIR)/CM3/CoreSupport/*.c \ - $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x/*.c)) - -# Search path and source files for the ST stdperiph library -VPATH := $(VPATH):$(STDPERIPH_DIR)/src -STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) - -# board-specific source files -VPATH := $(VPATH):$(BOARD_DIR) -BOARD_C_SRC = flash.c -BOARD_CXX_SRC = breezy_board.cpp \ - main.cpp - -# Hardware Driver Source Files -BREEZY_SRC = drv_gpio.c \ - drv_i2c.c \ - drv_adc.c \ - drv_spi.c \ - drv_pwm.c \ - drv_system.c \ - drv_serial.c \ - drv_uart.c \ - drv_timer.c \ - drv_mpu6050.c \ - drv_ms4525.c \ - drv_mb1242.c \ - drv_ms5611.c \ - drv_bmp280.c \ - drv_hmc5883l.c - -# Add F1 C Sources -CSOURCES = $(CMSIS_SRC) \ - $(STDPERIPH_SRC) \ - $(BREEZY_SRC) \ - $(BOARD_C_SRC) - -# Add F1 CXX Sources -CXXSOURCES += $(BOARD_CXX_SRC) - -# Add F1 Include Directories -INCLUDE_DIRS += $(BREEZY_DIR) \ - $(STDPERIPH_DIR)/inc \ - $(CMSIS_DIR)/CM3/CoreSupport \ - $(CMSIS_DIR)/CM3/DeviceSupport/ST/STM32F10x - -################################# -# Flags -################################# - -MCFLAGS=-mcpu=cortex-m3 -mthumb -DEFS+=-DTARGET_STM32F10X_MD -D__CORTEX_M3 -DWORDS_STACK_SIZE=200 -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -CFLAGS+=$(MCFLAGS) $(OPTIMIZE) $(DEFS) $(addprefix -I,$(INCLUDE_DIRS)) -CXXFLAGS+=$(MCFLAGS) $(OPTIMIZE) $(addprefix -I,$(INCLUDE_DIRS)) -LDFLAGS =-T $(LDSCRIPT) $(MCFLAGS) -lm -lc --specs=nano.specs --specs=rdimon.specs $(ARCH_FLAGS) $(LTO_FLAGS) $(DEBUG_FLAGS) -static -Wl,-gc-sections - -################################# -# Build -################################# -$(TARGET_BIN): $(TARGET_HEX) - $(CP) -I ihex -O binary $< $@ - -$(TARGET_HEX): $(TARGET_ELF) - $(CP) -O ihex --set-start 0x8000000 $< $@ - -$(TARGET_ELF): $(OBJECTS) - $(CXX) -o $@ $^ $(LDFLAGS) - $(SIZE) $(TARGET_ELF) - -$(BIN_DIR)/$(TARGET)/%.o: %.cpp - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CXX) -c -o $@ $(CXXFLAGS) $< - -$(BIN_DIR)/$(TARGET)/%.o: %.c - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(CFLAGS) $< - -$(BIN_DIR)/$(TARGET)/%.o: %.s - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(CFLAGS) $< - - -################################# -# Recipes -################################# -.PHONY: all flash clean - -clean: - rm -rf $(OBJECTS) $(OBJECT_DIR) - -flash: $(TARGET_HEX) - stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 921600 $(SERIAL_DEVICE) diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp deleted file mode 100644 index 413c501d..00000000 --- a/boards/breezy/breezy_board.cpp +++ /dev/null @@ -1,310 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" - -extern "C" { -#include "flash.h" - -#include -extern void SetSysClock(bool overclock); -} - -#include "breezy_board.h" - -namespace rosflight_firmware -{ -BreezyBoard::BreezyBoard() {} - -void BreezyBoard::init_board() -{ - // Configure clock, this figures out HSE for hardware autodetect - SetSysClock(0); - systemInit(); - _board_revision = 2; -} - -void BreezyBoard::board_reset(bool bootloader) { systemReset(bootloader); } - -// clock - -uint32_t BreezyBoard::clock_millis() { return millis(); } - -uint64_t BreezyBoard::clock_micros() { return micros(); } - -void BreezyBoard::clock_delay(uint32_t milliseconds) { delay(milliseconds); } - -// serial - -void BreezyBoard::serial_init(uint32_t baud_rate, uint32_t dev) -{ - (void) dev; - Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX); -} - -void BreezyBoard::serial_write(const uint8_t * src, size_t len) -{ - for (size_t i = 0; i < len; i++) { serialWrite(Serial1, src[i]); } -} - -uint16_t BreezyBoard::serial_bytes_available() { return serialTotalBytesWaiting(Serial1); } - -uint8_t BreezyBoard::serial_read() { return serialRead(Serial1); } - -void BreezyBoard::serial_flush() { return; } - -// sensors - -void BreezyBoard::sensors_init() -{ - // Initialize I2c - i2cInit(I2CDEV_2); - - while (millis() < 50) - ; - - i2cWrite(0, 0, 0); - if (bmp280_init()) baro_type = BARO_BMP280; - else if (ms5611_init()) - baro_type = BARO_MS5611; - - hmc5883lInit(); - mb1242_init(); - ms4525_init(); - - // IMU - uint16_t acc1G; - mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision); - _accel_scale = 9.80665f / acc1G; -} - -uint16_t BreezyBoard::num_sensor_errors() { return i2cGetErrorCounter(); } - -bool BreezyBoard::new_imu_data() { return mpu6050_new_data(); } - -bool BreezyBoard::imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) -{ - volatile int16_t gyro_raw[3], accel_raw[3]; - volatile int16_t raw_temp; - mpu6050_async_read_all(accel_raw, &raw_temp, gyro_raw, time_us); - - accel[0] = accel_raw[0] * _accel_scale; - accel[1] = -accel_raw[1] * _accel_scale; - accel[2] = -accel_raw[2] * _accel_scale; - - gyro[0] = gyro_raw[0] * _gyro_scale; - gyro[1] = -gyro_raw[1] * _gyro_scale; - gyro[2] = -gyro_raw[2] * _gyro_scale; - - (*temperature) = (float) raw_temp / 340.0f + 36.53f; - - if (accel[0] == 0 && accel[1] == 0 && accel[2] == 0) { - return false; - } else - return true; -} - -void BreezyBoard::imu_not_responding_error() -{ - // If the IMU is not responding, then we need to change where we look for the interrupt - _board_revision = (_board_revision < 4) ? 5 : 2; - sensors_init(); -} - -void BreezyBoard::mag_read(float mag[3]) -{ - // Convert to NED - hmc5883l_async_read(mag); -} - -bool BreezyBoard::mag_present() { return hmc5883l_present(); } - -void BreezyBoard::mag_update() { hmc5883l_request_async_update(); } - -void BreezyBoard::baro_update() -{ - if (baro_type == BARO_BMP280) bmp280_async_update(); - else if (baro_type == BARO_MS5611) - ms5611_async_update(); - else { - bmp280_async_update(); - ms5611_async_update(); - } -} - -void BreezyBoard::baro_read(float * pressure, float * temperature) -{ - if (baro_type == BARO_BMP280) { - bmp280_async_update(); - bmp280_async_read(pressure, temperature); - } else if (baro_type == BARO_MS5611) { - ms5611_async_update(); - ms5611_async_read(pressure, temperature); - } -} - -bool BreezyBoard::baro_present() -{ - if (baro_type == BARO_BMP280) return bmp280_present(); - else if (baro_type == BARO_MS5611) - return ms5611_present(); - else { - if (bmp280_present()) { - baro_type = BARO_BMP280; - return true; - } else if (ms5611_present()) { - baro_type = BARO_MS5611; - return true; - } - } - return false; -} - -bool BreezyBoard::diff_pressure_present() { return ms4525_present(); } - -void BreezyBoard::diff_pressure_update() { return ms4525_async_update(); } - -void BreezyBoard::diff_pressure_read(float * diff_pressure, float * temperature) -{ - ms4525_async_update(); - ms4525_async_read(diff_pressure, temperature); -} - -void BreezyBoard::sonar_update() -{ - if (sonar_type == SONAR_I2C || sonar_type == SONAR_NONE) mb1242_async_update(); - - // We don't need to actively update the pwm sonar -} - -bool BreezyBoard::sonar_present() -{ - if (sonar_type == SONAR_I2C) return mb1242_present(); - else if (sonar_type == SONAR_PWM) - return sonarPresent(); - else { - if (mb1242_present()) { - sonar_type = SONAR_I2C; - return true; - } else if (sonarPresent()) { - sonar_type = SONAR_PWM; - return true; - } - } - return false; -} - -float BreezyBoard::sonar_read() -{ - if (sonar_type == SONAR_I2C) { - mb1242_async_update(); - return mb1242_async_read(); - } else if (sonar_type == SONAR_PWM) - return sonarRead(6); - else - return 0.0f; -} - -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) -{ - (void) rc_type; // TODO SBUS is not supported on F1 - pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); -} - -void BreezyBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) -{ - pwm_refresh_rate_ = refresh_rate; - pwm_idle_pwm_ = idle_pwm; - pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); -} - -void BreezyBoard::pwm_disable() -{ - pwm_refresh_rate_ = 50; - pwm_idle_pwm_ = 0; - pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); -} - -float BreezyBoard::rc_read(uint8_t channel) { return (float) (pwmRead(channel) - 1000) / 1000.0; } - -void BreezyBoard::pwm_write(uint8_t channel, float value) -{ - pwmWriteMotor(channel, static_cast(value * 1000) + 1000); -} - -bool BreezyBoard::rc_lost() { return ((millis() - pwmLastUpdate()) > 40); } - -// non-volatile memory - -void BreezyBoard::memory_init() { initEEPROM(); } - -bool BreezyBoard::memory_read(void * dest, size_t len) { return readEEPROM(dest, len); } - -bool BreezyBoard::memory_write(const void * src, size_t len) { return writeEEPROM(src, len); } - -// GNSS is not supported on breezy boards -GNSSData BreezyBoard::gnss_read() { return {}; } - -// GNSS is not supported on breezy boards -GNSSFull BreezyBoard::gnss_full_read() { return {}; } - -// GNSS is not supported on breezy boards -bool BreezyBoard::gnss_has_new_data() { return false; } - -// LED - -void BreezyBoard::led0_on() { LED0_ON; } - -void BreezyBoard::led0_off() { LED0_OFF; } - -void BreezyBoard::led0_toggle() { LED0_TOGGLE; } - -void BreezyBoard::led1_on() { LED1_ON; } - -void BreezyBoard::led1_off() { LED1_OFF; } - -void BreezyBoard::led1_toggle() { LED1_TOGGLE; } - -} // namespace rosflight_firmware - -#pragma GCC diagnostic pop diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h deleted file mode 100644 index 3171f08c..00000000 --- a/boards/breezy/breezy_board.h +++ /dev/null @@ -1,180 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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_BREEZY_BOARD_H -#define ROSFLIGHT_FIRMWARE_BREEZY_BOARD_H - -#include -#include -#include - -extern "C" { -#include -} - -#include "board.h" -#include "sensors.h" - -namespace rosflight_firmware -{ -class BreezyBoard : public Board -{ -private: - serialPort_t * Serial1; - - int _board_revision = 2; - - float _accel_scale = 1.0; - float _gyro_scale = 1.0; - - enum - { - SONAR_NONE, - SONAR_I2C, - SONAR_PWM - }; - uint8_t sonar_type = SONAR_NONE; - - rc_type_t rc_type_ = RC_TYPE_PPM; - uint32_t pwm_refresh_rate_ = 490; - uint16_t pwm_idle_pwm_ = 1000; - enum - { - BARO_NONE, - BARO_BMP280, - BARO_MS5611 - }; - uint8_t baro_type = BARO_NONE; - - bool new_imu_data_; - uint64_t imu_time_us_; - -public: - BreezyBoard(); - - // setup - void init_board() override; - void board_reset(bool bootloader) override; - - // clock - uint32_t clock_millis() override; - uint64_t clock_micros() override; - void clock_delay(uint32_t milliseconds) override; - - // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t * src, size_t len) override; - uint16_t serial_bytes_available() override; - uint8_t serial_read() override; - void serial_flush() override; - - // sensors - void sensors_init() override; - uint16_t num_sensor_errors() override; - - bool new_imu_data() override; - bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) override; - void imu_not_responding_error() override; - - bool mag_present() override; - void mag_update() override; - void mag_read(float mag[3]) override; - - bool baro_present() override; - void baro_update() override; - void baro_read(float * pressure, float * temperature) override; - - bool diff_pressure_present() override; - void diff_pressure_update() override; - void diff_pressure_read(float * diff_pressure, float * temperature) override; - - bool sonar_present() override; - void sonar_update() override; - float sonar_read() override; - - bool gnss_present() override { return false; } - - void gnss_update() override { 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; - GNSSFull gnss_full_read() override; - - // PWM - // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) - void rc_init(rc_type_t rc_type) override; - bool rc_lost() override; - float rc_read(uint8_t channel) override; - - void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; - void pwm_disable() override; - void pwm_write(uint8_t channel, float value) override; - - // non-volatile memory - void memory_init() override; - bool memory_read(void * dest, size_t len) override; - bool memory_write(const void * src, size_t len) override; - - // LEDs - void led0_on() override; - void led0_off() override; - void led0_toggle() override; - - void led1_on() override; - void led1_off() override; - void led1_toggle() 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 - -#endif // ROSFLIGHT_FIRMWARE_BREEZY_BOARD_H diff --git a/boards/breezy/breezystm32 b/boards/breezy/breezystm32 deleted file mode 160000 index d564c198..00000000 --- a/boards/breezy/breezystm32 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d564c1987415dd9764069a9591e15d5c3d1759de diff --git a/boards/breezy/flash.c b/boards/breezy/flash.c deleted file mode 100644 index 1bf4565f..00000000 --- a/boards/breezy/flash.c +++ /dev/null @@ -1,86 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ -#include - -#include - -#include "flash.h" - -void initEEPROM(void) -{ - // make sure (at compile time) that config struct doesn't overflow allocated flash pages - // ct_assert(sizeof(_params) < CONFIG_SIZE); -} - -static uint8_t compute_checksum(const void * addr, size_t len) -{ - const uint8_t *p; - uint8_t chk = 0; - - for (p = (const uint8_t *)addr; p < ((const uint8_t *)addr + len); p++) - chk ^= *p; - - return chk; -} - -bool readEEPROM(void * dest, size_t len) -{ - memcpy(dest, (char *)FLASH_WRITE_ADDR, len); - return true; -} - -bool writeEEPROM(const void * src, size_t len) -{ - FLASH_Status status; - - // write it - FLASH_Unlock(); - for (unsigned int tries = 3; tries; tries--) - { - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); - - for(int i = 0; i < NUM_PAGES; i++) - status = FLASH_ErasePage(FLASH_WRITE_ADDR + i * FLASH_PAGE_SIZE); - - for (unsigned int i = 0; i < len && status == FLASH_COMPLETE; i += 4) - status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)src + i)); - - if(status == FLASH_COMPLETE) - break; - } - FLASH_Lock(); - - // Flash write failed - just die now - if (status != FLASH_COMPLETE || compute_checksum(src, len) != compute_checksum((uint32_t*)FLASH_WRITE_ADDR, len)) - return false; - - return true; -} diff --git a/boards/breezy/flash.h b/boards/breezy/flash.h deleted file mode 100644 index 072ee9b8..00000000 --- a/boards/breezy/flash.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ -#pragma once - -#include -#include -#include -#include - -// #define ASSERT_CONCAT_(a, b) a##b -// #define ASSERT_CONCAT(a, b) ASSERT_CONCAT_(a, b) -// #define ct_assert(e) enum { ASSERT_CONCAT(assert_line_, __LINE__) = 1/(!!(e)) } - -// define this symbol to increase or decrease flash size. not rely on flash_size_register. -#ifndef FLASH_PAGE_COUNT -#define FLASH_PAGE_COUNT 128 -#endif - -#define FLASH_PAGE_SIZE ((uint16_t) 0x400) -#define NUM_PAGES 3 -// if sizeof(_params) is over this number, compile-time error will occur. so, need to add another -// page to config data. -// TODO compile time check is currently disabled -#define CONFIG_SIZE (FLASH_PAGE_SIZE * NUM_PAGES) - -// static const uint8_t EEPROM_CONF_VERSION = 76; -// static uint32_t enabledSensors = 0; -// static void resetConf(void); -static const uint32_t FLASH_WRITE_ADDR = - 0x08000000 + (FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - (CONFIG_SIZE / 1024))); - -/** - * @brief Initialize Flash - */ -void initEEPROM(void); - -/** - * @brief Read data from Flash - * @param dest The memory address to copy the data to - * @param len The number of bytes to copy - * @returns true if the read was successful, false otherwise - */ -bool readEEPROM(void * dest, size_t len); - -/** - * @brief Write data to Flash - * @param src The memory address to copy data from - * @param len The number of bytes to copy - * @returns true if the write was successful, false otherwise - */ -bool writeEEPROM(const void * src, size_t len); diff --git a/boards/breezy/main.cpp b/boards/breezy/main.cpp deleted file mode 100644 index 195138f8..00000000 --- a/boards/breezy/main.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, 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. - */ - -#include "breezy_board.h" -#include "mavlink.h" - -#include "rosflight.h" - -int main() -{ - rosflight_firmware::BreezyBoard board; - board.init_board(); - rosflight_firmware::Mavlink mavlink(board); - rosflight_firmware::ROSflight firmware(board, mavlink); - - firmware.init(); - - while (true) { firmware.run(); } - return 0; -} From e2068855299e137ec015c8af88b84821e3062d2b Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:15:00 -0700 Subject: [PATCH 03/19] added varmint repo as submodule --- .gitmodules | 3 +++ boards/varmint | 1 + 2 files changed, 4 insertions(+) create mode 160000 boards/varmint diff --git a/.gitmodules b/.gitmodules index 9ee88d00..8f323972 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "comms/mavlink/v1.0"] path = comms/mavlink/v1.0 url = https://github.com/rosflight/mavlink_c_library.git +[submodule "boards/varmint"] + path = boards/varmint + url = https://github.com/rosflight/varmint.git diff --git a/boards/varmint b/boards/varmint new file mode 160000 index 00000000..6a111f51 --- /dev/null +++ b/boards/varmint @@ -0,0 +1 @@ +Subproject commit 6a111f511c81823ffd1fda61d3bf2541b40bce3b From 8afb46098b072aa80fe65bd4b1a459c0c0e8ebd8 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:15:54 -0700 Subject: [PATCH 04/19] updated compilation to CMake --- CMakeLists.txt | 72 ++++++++++++++++++++++++++++ Makefile | 86 ---------------------------------- scripts/rosflight.mk | 109 ------------------------------------------- 3 files changed, 72 insertions(+), 195 deletions(-) create mode 100644 CMakeLists.txt delete mode 100644 Makefile delete mode 100644 scripts/rosflight.mk diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..d92f5378 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,72 @@ +### project settings ### + +cmake_minimum_required(VERSION 3.8) +project(rosflight_firmware C CXX ASM) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release") +endif() + +add_compile_options(-Wall) + + +### git ### + +# clone mavlink submodule if it is missing +set(MAVLINK_SUBMODULE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/comms/mavlink/v1.0") +if(NOT EXISTS "${MAVLINK_SUBMODULE_DIR}/.git") + message(STATUS "MAVlink submodule not found at ${MAVLINK_SUBMODULE_DIR}") + execute_process( + COMMAND git submodule update --init --recursive + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) +endif() + +# get version info +execute_process(COMMAND git rev-parse --short=8 HEAD + OUTPUT_VARIABLE GIT_VERSION_HASH + OUTPUT_STRIP_TRAILING_WHITESPACE + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) +execute_process(COMMAND git describe --tags --abbrev=8 --always --dirty --long + OUTPUT_VARIABLE GIT_VERSION_STRING + OUTPUT_STRIP_TRAILING_WHITESPACE + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) +if("${GIT_VERSION_STRING}" STREQUAL "") + set(GIT_VERSION_STRING "undefined") +endif() +if("${GIT_VERSION_HASH}" STREQUAL "") + set(GIT_VERSION_HASH "0") +endif() + + +### source files ### + +include_directories( + include + include/interface + lib + comms/mavlink + comms/mavlink/v1.0 + comms/mavlink/v1.0/common + comms/mavlink/v1.0/rosflight +) + +file(GLOB_RECURSE ROSFLIGHT_SOURCES + "src/*.cpp" + "lib/turbomath/turbomath.cpp" + "comms/mavlink/mavlink.cpp" +) + + +### select boards to compile ### + +option(BUILD_VARMINT "Build the varmint board target" ON) + +if(BUILD_VARMINT) + message(STATUS "Selecting varmint board target") + add_subdirectory(boards/varmint) +endif() + diff --git a/Makefile b/Makefile deleted file mode 100644 index 160296d7..00000000 --- a/Makefile +++ /dev/null @@ -1,86 +0,0 @@ -################################################################################ -# -# Copyright (c) 2017, James Jackson and Daniel Koch, 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. - -################################# -# GNU ARM Embedded Toolchain -################################# -BOARD ?= REVO - -# Debugger options, must be empty or GDB -DEBUG ?= - -# Serial port/device for flashing -SERIAL_DEVICE ?= /dev/ttyACM0 - -# Set proper number of jobs for the computer -PARALLEL_JOBS := $(shell grep -c ^processor /proc/cpuinfo) - -################################# -# Board Selection -################################# -# List of valid boards (update with new boards) -VALID_F1_BOARDS = NAZE -VALID_F4_BOARDS = REVO - -# Make sure that the supplied board is supported, and if so, -# set the proper board directory -ifeq ($(BOARD),$(filter $(BOARD),$(VALID_F4_BOARDS))) -BOARD_DIR=boards/airbourne -endif - -ifeq ($(BOARD),$(filter $(BOARD),$(VALID_F1_BOARDS))) -BOARD_DIR=boards/breezy -endif - -ifeq ($(BOARD_DIR),) -$(info Invalid BOARD: $(BOARD)) -$(info =================================) -$(info VALID F1 BOARDS:) -$(info $(VALID_F1_BOARDS)) -$(info =================================) -$(info VALID F4 BOARDS:) -$(info $(VALID_F4_BOARDS)) -$(info =================================) -else -$(info Building ROSflight $(BOARD_DIR)) -endif - -.PHONY: all flash clean - -all: - cd $(BOARD_DIR) && make -j$(PARALLEL_JOBS) -l$(PARALLEL_JOBS) DEBUG=$(DEBUG) SERIAL_DEVICE=$(SERIAL_DEVICE) - -clean: - cd boards/airbourne && make clean - cd boards/breezy && make clean - -flash: - cd $(BOARD_DIR) && make -j$(PARALLEL_JOBS) -l$(PARALLEL_JOBS) DEBUG=$(DEBUG) SERIAL_DEVICE=$(SERIAL_DEVICE) flash diff --git a/scripts/rosflight.mk b/scripts/rosflight.mk deleted file mode 100644 index 20558379..00000000 --- a/scripts/rosflight.mk +++ /dev/null @@ -1,109 +0,0 @@ -################################# -# GNU ARM Embedded Toolchain -################################# -CC=arm-none-eabi-gcc -CXX=arm-none-eabi-g++ -LD=arm-none-eabi-ld -AR=arm-none-eabi-ar -AS=arm-none-eabi-as -CP=arm-none-eabi-objcopy -OD=arm-none-eabi-objdump -NM=arm-none-eabi-nm -SIZE=arm-none-eabi-size -A2L=arm-none-eabi-addr2line - -################################# -# DIRECTORIES -################################# -ROSFLIGHT_DIR = ../.. -TURBOMATH_DIR = $(ROSFLIGHT_DIR)/lib/turbomath -MAVLINK_DIR = $(ROSFLIGHT_DIR)/comms/mavlink - -################################# -# SOURCE FILES -################################# - -# MAVLink source files -VPATH := $(VPATH):$(MAVLINK_DIR) -MAVLINK_SRC = mavlink.cpp - -# ROSflight source files -VPATH := $(VPATH):$(ROSFLIGHT_DIR):$(ROSFLIGHT_DIR)/src -ROSFLIGHT_SRC = rosflight.cpp \ - param.cpp \ - sensors.cpp \ - state_manager.cpp \ - estimator.cpp \ - controller.cpp \ - comm_manager.cpp \ - command_manager.cpp \ - rc.cpp \ - mixer.cpp - -# Math Source Files -VPATH := $(VPATH):$(TURBOMATH_DIR) -MATH_SRC = turbomath.cpp - -# List of Common C++ Source Files -CXXSOURCES = $(ROSFLIGHT_SRC) \ - $(MATH_SRC) \ - $(MAVLINK_SRC) - -# No C sources in common -CSOURCES = - -# Common Include Files -INCLUDE_DIRS = $(ROSFLIGHT_DIR)/include \ - $(ROSFLIGHT_DIR)/lib \ - $(MAVLINK_DIR) - -################################# -# BUILD TYPE -################################# -ifeq ($(DEBUG), GDB) -DEBUG_FLAGS = -ggdb -OPTIMIZE = -O0 -$(info ***** Building with Debug Symbols *****) -BUILD_TYPE=Debug -else -DEBUG_FLAGS = -ggdb -OPTIMIZE = -O2 -COMPILE_FLAGS = -flto -LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE) -BUILD_TYPE=Release -endif - - -################################# -# VERSION CONTROL -################################# -GIT_VERSION_HASH := $(shell git rev-parse --short=8 HEAD) -GIT_VERSION_STRING := $(shell git describe --tags --abbrev=8 --always --dirty --long) -GIT_VARS := -DGIT_VERSION_HASH=0x$(GIT_VERSION_HASH) -DGIT_VERSION_STRING=\"$(GIT_VERSION_STRING)\" - -################################# -# Object List -################################# -OBJECTS=$(addsuffix .o,$(addprefix $(BIN_DIR)/$(TARGET)/,$(basename $(ASOURCES)))) -OBJECTS+=$(addsuffix .o,$(addprefix $(BIN_DIR)/$(TARGET)/,$(basename $(CSOURCES)))) -OBJECTS+=$(addsuffix .o,$(addprefix $(BIN_DIR)/$(TARGET)/,$(basename $(CXXSOURCES)))) - -################################# -# Target Output Files -################################# -TARGET_ELF=$(BIN_DIR)/$(TARGET)_$(BOARD)_$(BUILD_TYPE).elf -TARGET_HEX=$(BIN_DIR)/$(TARGET)_$(BOARD)_$(BUILD_TYPE).hex -TARGET_BIN=$(BIN_DIR)/$(TARGET)_$(BOARD)_$(BUILD_TYPE).bin - -################################# -# Common Flags -################################# -DEFS = -DTARGET_$(BOARD) $(GIT_VARS) -CXX_STRICT_FLAGS += -pedantic -pedantic-errors -Werror -Wall -Wextra \ - -Wcast-align -Wcast-qual -Wdisabled-optimization -Wformat=2 -Wlogical-op -Wmissing-include-dirs \ - -Wredundant-decls -Wshadow -Wstrict-overflow=2 -Wswitch -Wundef -Wunused -Wvariadic-macros \ - -Wctor-dtor-privacy -Wnoexcept -Wold-style-cast -Woverloaded-virtual -Wsign-promo -Wstrict-null-sentinel -FILE_SIZE_FLAGS += -ffunction-sections -fdata-sections -fno-exceptions -CXX_FILE_SIZE_FLAGS =-c $(FILE_SIZE_FLAGS) -fno-rtti -CFLAGS = -c $(DEFS) $(DEBUG_FLAGS) $(FILE_SIZE_FLAGS) -std=c99 -CXXFLAGS = -c $(DEFS) $(DEBUG_FLAGS) $(CXX_FILE_SIZE_FLAGS) $(CXX_STRICT_FLAGS) -std=c++11 From 624fbf5278b522e28e39c294e4b0144e3ba169e5 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:16:11 -0700 Subject: [PATCH 05/19] updated scripts for varmint board --- scripts/fix-code-style.sh | 2 +- scripts/run_tests.sh | 20 +++++++++----------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/scripts/fix-code-style.sh b/scripts/fix-code-style.sh index 934e476c..6f48a55c 100755 --- a/scripts/fix-code-style.sh +++ b/scripts/fix-code-style.sh @@ -7,5 +7,5 @@ echo #SCRIPTPATH cd $SCRIPTPATH/.. find . -iname "*.h" -o -iname "*.hpp" -o -iname "*.cpp" | \ -grep -Ev "^(./comms/mavlink/v1.0/|./boards/airbourne/airbourne/|./boards/breezy/breezystm32|./.git|./test/build)" | \ + grep -Ev "^(./comms/mavlink/v1.0/|./.git|./boards/varmint/lib/)" | \ xargs clang-format -i diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index b5c0980b..2e88bbfc 100755 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -19,27 +19,25 @@ function print_result() { BASENAME=`basename "$PWD"` if [ $BASENAME == "scripts" ]; then - cd .. + cd .. fi -echo_blue "Test 1: Build F1 firmware" -make clean BOARD=NAZE -make BOARD=NAZE +echo_blue "Test 1: Build varmint firmware" +rm -rf build +mkdir build +cd build +cmake .. -DBUILD_VARMINT=true -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 print_result $? +cd .. -echo_blue "Test 2: Build F4 firmware" -make clean BOARD=REVO -make BOARD=REVO -print_result $? - -echo_blue "Test 3: Build test suite" +echo_blue "Test 2: Build test suite" mkdir -p test/build cd test/build rm -rf * cmake .. -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 print_result $? -echo_blue "Test 4: Run test suite" +echo_blue "Test 3: Run test suite" ./unit_tests print_result $? From 0c6f77afa20b74242d44f25b976b5b11b6868f19 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:24:13 -0700 Subject: [PATCH 06/19] updated github actions for varmint board --- .github/workflows/f4_firmware.yml | 19 ------------------- .github/workflows/pre-release.yml | 11 ++++------- .github/workflows/unit_tests.yml | 16 ++++++++-------- .../{f1_firmware.yml => varmint_firmware.yml} | 6 +++--- 4 files changed, 15 insertions(+), 37 deletions(-) delete mode 100644 .github/workflows/f4_firmware.yml rename .github/workflows/{f1_firmware.yml => varmint_firmware.yml} (73%) diff --git a/.github/workflows/f4_firmware.yml b/.github/workflows/f4_firmware.yml deleted file mode 100644 index 7c4565a3..00000000 --- a/.github/workflows/f4_firmware.yml +++ /dev/null @@ -1,19 +0,0 @@ -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 apt -y install gcc-arm-none-eabi - - 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 index e7a26218..bad373a9 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -4,7 +4,7 @@ name: "pre-release" on: push: branches: - - "master" + - "main" jobs: pre-release: @@ -19,10 +19,8 @@ jobs: run: sudo apt -y install gcc-arm-none-eabi - 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 + - name: build varmint + run: mkdir build && cd build && cmake .. -DBUILD_VARMINT=true && make - uses: "marvinpinto/action-automatic-releases@latest" with: @@ -31,5 +29,4 @@ jobs: prerelease: true title: "Development Build" files: | - boards/airbourne/build/rosflight_REVO_Release.bin - boards/breezy/build/rosflight_NAZE_Release.hex + build/varmint.hex diff --git a/.github/workflows/unit_tests.yml b/.github/workflows/unit_tests.yml index 3da1ae37..d68d7a0c 100644 --- a/.github/workflows/unit_tests.yml +++ b/.github/workflows/unit_tests.yml @@ -6,7 +6,7 @@ jobs: build: runs-on: ubuntu-latest - + steps: - uses: actions/checkout@v2 - name: clone @@ -16,20 +16,20 @@ jobs: - name: install gtest run: | cd /usr/src/gtest - sudo cmake CMakeLists.txt - sudo make + sudo cmake CMakeLists.txt + sudo make sudo cp ./lib/libgtest*.a /usr/lib - name: cmake run: | - cd test - mkdir build - cd build + cd test + mkdir build + cd build cmake .. -DCMAKE_BUILD_TYPE=Release - name: make run: | - cd test/build + cd test/build make - name: test run: | - cd test/build + cd test/build ./unit_tests diff --git a/.github/workflows/f1_firmware.yml b/.github/workflows/varmint_firmware.yml similarity index 73% rename from .github/workflows/f1_firmware.yml rename to .github/workflows/varmint_firmware.yml index 047a3f45..ed4f87e1 100644 --- a/.github/workflows/f1_firmware.yml +++ b/.github/workflows/varmint_firmware.yml @@ -1,4 +1,4 @@ -name: F1 Firmware +name: Varmint Firmware on: [push] @@ -15,5 +15,5 @@ jobs: run: sudo apt -y install gcc-arm-none-eabi - name: check toolchain run: arm-none-eabi-gcc --version - - name: make - run: make BOARD=NAZE -j4 -l4 + - name: build + run: mkdir build && cd build && cmake .. -DBUILD_VARMINT=true && make From 4956a06f8dec8430ffa6eeac29405138fc77c6b1 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:24:39 -0700 Subject: [PATCH 07/19] updated .gitignore --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 2c221aa2..0b123f76 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,7 @@ .vscode/ *.o *.html -/boards/*/build/ +/build/ /test/build/ /site/ *.idea* From aea299b5c9da47d3c625a2dd7e71aaecfb9b8fca Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 09:40:59 -0700 Subject: [PATCH 08/19] updated submodule with working cmake --- boards/varmint | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/varmint b/boards/varmint index 6a111f51..fd182876 160000 --- a/boards/varmint +++ b/boards/varmint @@ -1 +1 @@ -Subproject commit 6a111f511c81823ffd1fda61d3bf2541b40bce3b +Subproject commit fd1828766db02dc41cf92d0b771ca4e2e26d8fd1 From f05d8b83c16d4aaf2ab3aac27d75ead08b7ed354 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 12:36:42 -0700 Subject: [PATCH 09/19] Added qos argument to serial_write in board.h --- .gitignore | 3 ++- boards/varmint | 2 +- comms/mavlink/mavlink.cpp | 8 ++++---- comms/mavlink/mavlink.h | 2 +- include/board.h | 3 ++- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index 0b123f76..6e95683e 100644 --- a/.gitignore +++ b/.gitignore @@ -15,7 +15,8 @@ /build/ /test/build/ /site/ -*.idea* +.idea/ +cmake-build* *.map scripts/parameter-descriptions.md diff --git a/boards/varmint b/boards/varmint index fd182876..c6d41446 160000 --- a/boards/varmint +++ b/boards/varmint @@ -1 +1 @@ -Subproject commit fd1828766db02dc41cf92d0b771ca4e2e26d8fd1 +Subproject commit c6d41446c2ea6620c0e7481d82cb1ad984cd0380 diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 1db2b803..aa18e516 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -139,7 +139,7 @@ void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath mavlink_message_t msg; mavlink_msg_small_imu_pack(system_id, compid_, &msg, timestamp_us, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, temperature); - send_message(msg); + send_message(msg, 0); } void Mavlink::send_gnss(uint8_t system_id, const GNSSData & data) { @@ -296,7 +296,7 @@ void Mavlink::send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) { mavlink_message_t msg; mavlink_msg_timesync_pack(system_id, compid_, &msg, tc1, ts1); - send_message(msg); + send_message(msg, 1); } void Mavlink::send_version(uint8_t system_id, const char * const version) @@ -320,12 +320,12 @@ void Mavlink::send_battery_status(uint8_t system_id, float voltage, float curren send_message(msg); } -void Mavlink::send_message(const mavlink_message_t & msg) +void Mavlink::send_message(const mavlink_message_t & msg, uint8_t qos) { if (initialized_) { uint8_t data[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(data, &msg); - board_.serial_write(data, len); + board_.serial_write(data, len, qos); } } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index f2e017f3..3fb93248 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -96,7 +96,7 @@ class Mavlink : public CommLinkInterface inline void set_listener(ListenerInterface * listener) override { listener_ = listener; } private: - void send_message(const mavlink_message_t & msg); + void send_message(const mavlink_message_t & msg, uint8_t qos = UINT8_MAX); void handle_msg_param_request_list(const mavlink_message_t * const msg); void handle_msg_param_request_read(const mavlink_message_t * const msg); diff --git a/include/board.h b/include/board.h index dc173617..2b350b97 100644 --- a/include/board.h +++ b/include/board.h @@ -61,7 +61,8 @@ class Board // serial virtual void serial_init(uint32_t baud_rate, uint32_t dev) = 0; - virtual void serial_write(const uint8_t * src, size_t len) = 0; + // qos defines the 'priority' of the packet, with 0 being the highest + virtual void serial_write(const uint8_t * src, size_t len, uint8_t qos) = 0; virtual uint16_t serial_bytes_available() = 0; virtual uint8_t serial_read() = 0; virtual void serial_flush() = 0; From 8be7e65b96c48aaba65f837893307569ffb2bdb9 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 15:28:44 -0700 Subject: [PATCH 10/19] Moved main.cpp from boards to firmware src --- boards/varmint | 2 +- src/main.cpp | 70 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 src/main.cpp diff --git a/boards/varmint b/boards/varmint index c6d41446..c3cbf7c8 160000 --- a/boards/varmint +++ b/boards/varmint @@ -1 +1 @@ -Subproject commit c6d41446c2ea6620c0e7481d82cb1ad984cd0380 +Subproject commit c3cbf7c8e153861f70de7d940b5f8601c2f5678a diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 00000000..e6485881 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * File : main.cpp + * Date : Oct 5, 2023 + ****************************************************************************** + * + * Copyright (c) 2023, AeroVironment, Inc. + * 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 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. + * + ****************************************************************************** + **/ + +#include +#include +#include + +extern Varmint varmint; + +#ifdef __cplusplus +extern "C" { +#endif + +int main(void); + +#ifdef __cplusplus +} +#endif + +/** + * @fn int main(void) + * @brief Program Start + */ +int main(void) +{ + // Rosflight board code + varmint.init_board(); + + // Rosflight base code + rosflight_firmware::Mavlink mavlink(varmint); + rosflight_firmware::ROSflight firmware(varmint, mavlink); + + firmware.init(); + + while (true) { firmware.run(); } +} From 9887a0771ec7b7fb8a0bad9b4654a51c1aad5929 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 16:00:25 -0700 Subject: [PATCH 11/19] Added logic for selecting which board to compile --- CMakeLists.txt | 18 +++++++++--------- boards/varmint | 2 +- src/main.cpp | 26 ++++++++++++++++++-------- 3 files changed, 28 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d92f5378..c2d9e137 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,9 +10,6 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release") endif() -add_compile_options(-Wall) - - ### git ### # clone mavlink submodule if it is missing @@ -41,7 +38,6 @@ if("${GIT_VERSION_HASH}" STREQUAL "") set(GIT_VERSION_HASH "0") endif() - ### source files ### include_directories( @@ -60,13 +56,17 @@ file(GLOB_RECURSE ROSFLIGHT_SOURCES "comms/mavlink/mavlink.cpp" ) - ### select boards to compile ### -option(BUILD_VARMINT "Build the varmint board target" ON) +option(BUILD_VARMINT "Build the varmint board target" OFF) +option(BUILD_TEST "Build the test board target" OFF) if(BUILD_VARMINT) - message(STATUS "Selecting varmint board target") + message("===== Selecting varmint board target. =====") add_subdirectory(boards/varmint) -endif() - +elseif(BUILD_TEST) + message("===== Selecting test board target. ======") +# add_subdirectory() +else() + message(FATAL_ERROR "No board selected! Please select the varmint or test board with -DBUILD_VARMINT=true or -DBUILD_TEST=true.") +endif () \ No newline at end of file diff --git a/boards/varmint b/boards/varmint index c3cbf7c8..a66cacd3 160000 --- a/boards/varmint +++ b/boards/varmint @@ -1 +1 @@ -Subproject commit c3cbf7c8e153861f70de7d940b5f8601c2f5678a +Subproject commit a66cacd33d69603267120dada41252f62b618e65 diff --git a/src/main.cpp b/src/main.cpp index e6485881..7346bec5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -35,18 +35,22 @@ ****************************************************************************** **/ -#include #include #include -extern Varmint varmint; +// Select which board implementation to include based on cmake variable +#ifdef BUILD_VARMINT_BOARD +#include +extern Varmint varmint; // TODO: Eliminate global variable +#endif +#ifdef BUILD_TEST_BOARD +#include +#endif #ifdef __cplusplus extern "C" { #endif - int main(void); - #ifdef __cplusplus } #endif @@ -57,12 +61,18 @@ int main(void); */ int main(void) { - // Rosflight board code - varmint.init_board(); + // Select which board implementation to instantiate based on cmake variable +#ifdef BUILD_VARMINT_BOARD + rosflight_firmware::Board * board = &varmint; +#endif +#ifdef BUILD_TEST_BOARD + return 0; +#endif // Rosflight base code - rosflight_firmware::Mavlink mavlink(varmint); - rosflight_firmware::ROSflight firmware(varmint, mavlink); + board->init_board(); + rosflight_firmware::Mavlink mavlink(* board); + rosflight_firmware::ROSflight firmware(* board, mavlink); firmware.init(); From 2082c65b0c9991b037a90232e94941cc5a23a06e Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 16:32:24 -0700 Subject: [PATCH 12/19] internal submodule changes --- boards/varmint | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/varmint b/boards/varmint index a66cacd3..59664d95 160000 --- a/boards/varmint +++ b/boards/varmint @@ -1 +1 @@ -Subproject commit a66cacd33d69603267120dada41252f62b618e65 +Subproject commit 59664d9592de7d7323cbb156f750e7ed11b402bd From fb4c54f25e1c85d2e1e0af539455ae4347cf73b5 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 17:20:22 -0700 Subject: [PATCH 13/19] Added test board to cmake board selection --- CMakeLists.txt | 4 ++-- scripts/run_tests.sh | 12 ++++++------ src/main.cpp | 9 +++------ test/CMakeLists.txt | 32 +++----------------------------- test/test_board.cpp | 2 +- test/test_board.h | 2 +- 6 files changed, 16 insertions(+), 45 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2d9e137..1d977e77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ if(BUILD_VARMINT) add_subdirectory(boards/varmint) elseif(BUILD_TEST) message("===== Selecting test board target. ======") -# add_subdirectory() + add_subdirectory(test) else() - message(FATAL_ERROR "No board selected! Please select the varmint or test board with -DBUILD_VARMINT=true or -DBUILD_TEST=true.") + message(FATAL_ERROR "No board selected! Please select the varmint or test board with -DBUILD_VARMINT=TRUE or -DBUILD_TEST=TRUE.") endif () \ No newline at end of file diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index 2e88bbfc..f390c70a 100755 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -26,19 +26,19 @@ echo_blue "Test 1: Build varmint firmware" rm -rf build mkdir build cd build -cmake .. -DBUILD_VARMINT=true -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 +cmake .. -DBUILD_VARMINT=TRUE -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 print_result $? cd .. echo_blue "Test 2: Build test suite" -mkdir -p test/build -cd test/build -rm -rf * -cmake .. -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 +rm -rf build +mkdir build +cd build +cmake .. -DBUILD_TEST=TRUE -DCMAKE_BUILD_TYPE=RELEASE && make -j4 -l4 print_result $? echo_blue "Test 3: Run test suite" -./unit_tests +./test/unit_tests print_result $? diff --git a/src/main.cpp b/src/main.cpp index 7346bec5..e0f7914f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -43,9 +43,7 @@ #include extern Varmint varmint; // TODO: Eliminate global variable #endif -#ifdef BUILD_TEST_BOARD -#include -#endif +#ifndef BUILD_TEST_BOARD // Skip main function for gtest #ifdef __cplusplus extern "C" { @@ -65,9 +63,6 @@ int main(void) #ifdef BUILD_VARMINT_BOARD rosflight_firmware::Board * board = &varmint; #endif -#ifdef BUILD_TEST_BOARD - return 0; -#endif // Rosflight base code board->init_board(); @@ -78,3 +73,5 @@ int main(void) while (true) { firmware.run(); } } + +#endif // BUILD_TEST_BOARD diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5f2f5d12..85243bba 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 3.8) project(rosflight_tests) -# C++ standard -add_definitions(-std=c++17) - # Strict Compilation set(CMAKE_CXX_FLAGS "-pedantic -pedantic-errors -Werror -Wall -Wextra \ -Wcast-align -Wcast-qual -Wdisabled-optimization -Wformat=2 -Wlogical-op -Wmissing-include-dirs \ @@ -11,10 +8,6 @@ set(CMAKE_CXX_FLAGS "-pedantic -pedantic-errors -Werror -Wall -Wextra \ -Wctor-dtor-privacy -Wnoexcept -Wold-style-cast -Woverloaded-virtual -Wsign-promo -Wstrict-null-sentinel" ${CMAKE_CXX_FLAGS}) -# Git information -execute_process(COMMAND git rev-parse --short=8 HEAD OUTPUT_VARIABLE GIT_VERSION_HASH OUTPUT_STRIP_TRAILING_WHITESPACE) -execute_process(COMMAND git describe --tags --abbrev=8 --always --dirty --long OUTPUT_VARIABLE GIT_VERSION_STRING OUTPUT_STRIP_TRAILING_WHITESPACE) - add_definitions(-DGIT_VERSION_HASH=0x${GIT_VERSION_HASH}) add_definitions(-DGIT_VERSION_STRING=\"${GIT_VERSION_STRING}\") @@ -23,32 +16,12 @@ find_package(GTest REQUIRED) find_package(Threads REQUIRED) # Required for current gtest version, may be a bug with gtest? include_directories(${GTEST_INCLUDE_DIRS}) -include_directories(../include) -include_directories(../lib) -include_directories(../comms/mavlink) include_directories(${EIGEN3_INCLUDE_DIRS}) include_directories(/usr/include/eigen3) -set(ROSFLIGHT_SRC - ../src/rosflight.cpp - ../src/param.cpp - ../src/sensors.cpp - ../src/state_manager.cpp - ../src/estimator.cpp - ../src/controller.cpp - ../src/comm_manager.cpp - ../src/command_manager.cpp - ../src/rc.cpp - ../src/mixer.cpp - ../comms/mavlink/mavlink.cpp - ../lib/turbomath/turbomath.cpp - ) - - - +include_directories(.) add_executable(unit_tests - ${ROSFLIGHT_SRC} - common.h + ${ROSFLIGHT_SOURCES} common.cpp command_manager_test.cpp test_board.cpp @@ -59,3 +32,4 @@ add_executable(unit_tests parameters_test.cpp ) target_link_libraries(unit_tests ${GTEST_LIBRARIES} pthread) +target_compile_definitions(unit_tests PRIVATE BUILD_TEST_BOARD) diff --git a/test/test_board.cpp b/test/test_board.cpp index 3b7c9e7a..adfbc0e6 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -67,7 +67,7 @@ void testBoard::clock_delay(uint32_t milliseconds) {} // serial void testBoard::serial_init(uint32_t baud_rate, uint32_t dev) {} -void testBoard::serial_write(const uint8_t * src, size_t len) {} +void testBoard::serial_write(const uint8_t * src, size_t len, uint8_t qos) {} uint16_t testBoard::serial_bytes_available() { return 0; } uint8_t testBoard::serial_read() { return 0; } void testBoard::serial_flush() {} diff --git a/test/test_board.h b/test/test_board.h index cccd4f73..e61fa9da 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -62,7 +62,7 @@ class testBoard : public Board // serial void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t * src, size_t len) override; + void serial_write(const uint8_t * src, size_t len, uint8_t qos) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; void serial_flush() override; From 5c1b3c907e5b077aa2f1da84c116078d44c145bc Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 17:35:58 -0700 Subject: [PATCH 14/19] Updated gh actions with cmake changes --- .github/workflows/release.yml | 33 +++++++++++++++----------------- .github/workflows/unit_tests.yml | 9 ++++----- 2 files changed, 19 insertions(+), 23 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index a8eabe6b..2148bd8d 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -12,22 +12,19 @@ jobs: runs-on: "ubuntu-latest" steps: - - uses: actions/checkout@v2 - - name: checkout submodules - run: git submodule update --init --recursive - - name: install toolchain - run: sudo apt -y install gcc-arm-none-eabi - - 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: actions/checkout@v2 + - name: checkout submodules + run: git submodule update --init --recursive + - name: install toolchain + run: sudo apt -y install gcc-arm-none-eabi + - name: check toolchain + run: arm-none-eabi-gcc --version + - name: build varmint + run: mkdir build && cd build && cmake .. -DBUILD_VARMINT=true && make - - 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 + - uses: "marvinpinto/action-automatic-releases@latest" + with: + repo_token: "${{ secrets.GITHUB_TOKEN }}" + prerelease: false + files: | + build/varmint.hex diff --git a/.github/workflows/unit_tests.yml b/.github/workflows/unit_tests.yml index d68d7a0c..43f7fd79 100644 --- a/.github/workflows/unit_tests.yml +++ b/.github/workflows/unit_tests.yml @@ -21,15 +21,14 @@ jobs: sudo cp ./lib/libgtest*.a /usr/lib - name: cmake run: | - cd test mkdir build cd build - cmake .. -DCMAKE_BUILD_TYPE=Release + cmake .. -DBUILD_TEST=true -DCMAKE_BUILD_TYPE=Release - name: make run: | - cd test/build + cd build make - name: test run: | - cd test/build - ./unit_tests + cd /build + ./test/unit_tests From 91d254f24bc3c2c260bf1ccec7fa0419f15a6031 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 17:41:44 -0700 Subject: [PATCH 15/19] Misc changes to appease linter --- .github/workflows/unit_tests.yml | 2 +- src/main.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/unit_tests.yml b/.github/workflows/unit_tests.yml index 43f7fd79..b79b8fa5 100644 --- a/.github/workflows/unit_tests.yml +++ b/.github/workflows/unit_tests.yml @@ -30,5 +30,5 @@ jobs: make - name: test run: | - cd /build + cd build ./test/unit_tests diff --git a/src/main.cpp b/src/main.cpp index e0f7914f..047740ea 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -41,7 +41,7 @@ // Select which board implementation to include based on cmake variable #ifdef BUILD_VARMINT_BOARD #include -extern Varmint varmint; // TODO: Eliminate global variable +extern Varmint varmint; // TODO: Eliminate global variable #endif #ifndef BUILD_TEST_BOARD // Skip main function for gtest @@ -66,8 +66,8 @@ int main(void) // Rosflight base code board->init_board(); - rosflight_firmware::Mavlink mavlink(* board); - rosflight_firmware::ROSflight firmware(* board, mavlink); + rosflight_firmware::Mavlink mavlink(*board); + rosflight_firmware::ROSflight firmware(*board, mavlink); firmware.init(); From 3d5ad450cdcb4f16fbb744c529a9398533a93271 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 17:50:21 -0700 Subject: [PATCH 16/19] Removed git submodule checking from CMake Managing git repositories within CMake is considered bad practice, since CMake wasn't really build for that and it adds complexity to the CMakeLists.txt --- CMakeLists.txt | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d977e77..85a1ce71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,16 +12,6 @@ endif() ### git ### -# clone mavlink submodule if it is missing -set(MAVLINK_SUBMODULE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/comms/mavlink/v1.0") -if(NOT EXISTS "${MAVLINK_SUBMODULE_DIR}/.git") - message(STATUS "MAVlink submodule not found at ${MAVLINK_SUBMODULE_DIR}") - execute_process( - COMMAND git submodule update --init --recursive - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - ) -endif() - # get version info execute_process(COMMAND git rev-parse --short=8 HEAD OUTPUT_VARIABLE GIT_VERSION_HASH From 7403f1dbb2377d7c99eaf09cd30a9dc8c4e2f608 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jan 2024 17:58:21 -0700 Subject: [PATCH 17/19] Misc change to code style script --- scripts/fix-code-style.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/fix-code-style.sh b/scripts/fix-code-style.sh index 6f48a55c..cf1e2afc 100755 --- a/scripts/fix-code-style.sh +++ b/scripts/fix-code-style.sh @@ -7,5 +7,5 @@ echo #SCRIPTPATH cd $SCRIPTPATH/.. find . -iname "*.h" -o -iname "*.hpp" -o -iname "*.cpp" | \ - grep -Ev "^(./comms/mavlink/v1.0/|./.git|./boards/varmint/lib/)" | \ + grep -Ev "^(./comms/mavlink/v1.0|./.git|./boards)" | \ xargs clang-format -i From dc1840313f8bb8a4f9c1b20d2261beeaee467a6d Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 4 Jan 2024 11:02:36 -0700 Subject: [PATCH 18/19] Removed todo from main Initially had thought using extern was unnecessary, but due to how ST's code is structured it may be necessary. --- src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 047740ea..d3b69d73 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -41,7 +41,7 @@ // Select which board implementation to include based on cmake variable #ifdef BUILD_VARMINT_BOARD #include -extern Varmint varmint; // TODO: Eliminate global variable +extern Varmint varmint; #endif #ifndef BUILD_TEST_BOARD // Skip main function for gtest From dd2922a764b79a318d2c823589b705e0e006a538 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Thu, 4 Jan 2024 11:15:33 -0700 Subject: [PATCH 19/19] Update README.md --- README.md | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 99862e9e..ec721a3d 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,10 @@ # ROSflight -![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) +[![Unit Tests](https://github.com/rosflight/rosflight_firmware/actions/workflows/unit_tests.yml/badge.svg)](https://github.com/rosflight/rosflight_firmware/actions/workflows/unit_tests.yml) +[![Varmint Firmware](https://github.com/rosflight/rosflight_firmware/actions/workflows/varmint_firmware.yml/badge.svg)](https://github.com/rosflight/rosflight_firmware/actions/workflows/varmint_firmware.yml) -![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. +This is the core firmware library for ROSflight. ROSflight is a software architecture which uses a flight controller in tandem with a 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 companion computer and ROS. ROSflight is designed to accomplish the following objectives: