diff --git a/Makefile b/Makefile index e6cc6aaad..f2aac9ead 100644 --- a/Makefile +++ b/Makefile @@ -340,7 +340,7 @@ PACKAGE_FW_TARGETS += fw_oplinkmini PACKAGE_FW_TARGETS += fw_gpsplatinum PACKAGE_FW_TARGETS += fw_osd PACKAGE_FW_TARGETS += fw_revoproto -PACKAGE_FW_TARGETS += fw_spracingf3evo fw_spracingf3 fw_nucleof303re fw_pikoblx +PACKAGE_FW_TARGETS += fw_spracingf3evo fw_spracingf3 fw_nucleof303re fw_pikoblx fw_tinyfish # Rules to generate GCS resources used to embed firmware binaries into the GCS. # They are used later by the vehicle setup wizard to update board firmware. diff --git a/flight/Makefile b/flight/Makefile index 188e7ae68..1d20554f8 100644 --- a/flight/Makefile +++ b/flight/Makefile @@ -14,7 +14,7 @@ ALL_BOARDS += oplinkmini ALL_BOARDS += gpsplatinum ALL_BOARDS += osd ALL_BOARDS += discoveryf4bare -ALL_BOARDS += ccf3d spracingf3 spracingf3evo nucleof303re pikoblx +ALL_BOARDS += ccf3d spracingf3 spracingf3evo nucleof303re pikoblx tinyfish # SimPosix only builds on Linux ifeq ($(UNAME), Linux) ALL_BOARDS += simposix @@ -36,6 +36,7 @@ discoveryf4bare_short := 'df4b' gpsplatinum_short := 'gps9' nucleof303re_short := 'nf3r' pikoblx_short := 'piko' +tinyfish_short := 'tfsh' # Start out assuming that we'll build fw, bl and bu for all boards FW_BOARDS := $(ALL_BOARDS) diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c index 6ebbe0170..1bfc4aa15 100644 --- a/flight/pios/common/pios_mpu6000.c +++ b/flight/pios/common/pios_mpu6000.c @@ -724,8 +724,6 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp_p) // Rotate the sensor to OP convention. The datasheet defines X as towards the right // and Y as forward. OP convention transposes this. Also the Z is defined negatively // to our convention - - // Currently we only support rotations on top so switch X/Y accordingly switch (dev->cfg->orientation) { case PIOS_MPU6000_TOP_0DEG: queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X @@ -752,9 +750,38 @@ static bool PIOS_MPU6000_HandleData(uint32_t gyro_read_timestamp_p) queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X break; + case PIOS_MPU6000_BOTTOM_0DEG: + queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X + queue_data->sample[0].x = -1 - GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X + queue_data->sample[1].x = -1 - GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + break; + case PIOS_MPU6000_BOTTOM_90DEG: + queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X + queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X + break; + case PIOS_MPU6000_BOTTOM_180DEG: + queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X + queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y + queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X + queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y + break; + case PIOS_MPU6000_BOTTOM_270DEG: + queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y + queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X + queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y + queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X + break; + } + if((dev->cfg->orientation & PIOS_MPU6000_LOCATION_MASK) == PIOS_MPU6000_LOCATION_TOP) { + queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z)); + queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z)); + } else { + queue_data->sample[0].z = GET_SENSOR_DATA(mpu6000_data, Accel_Z); + queue_data->sample[1].z = GET_SENSOR_DATA(mpu6000_data, Gyro_Z); } - queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z)); - queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z)); const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature); // Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53 queue_data->temperature = 3653 + (temp * 100) / 340; diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h index 831c43ce2..ad0e56687 100644 --- a/flight/pios/inc/pios_mpu6000.h +++ b/flight/pios/inc/pios_mpu6000.h @@ -125,13 +125,25 @@ enum pios_mpu6000_accel_range { PIOS_MPU6000_ACCEL_16G = 0x18 }; -enum pios_mpu6000_orientation { // clockwise rotation from board forward - PIOS_MPU6000_TOP_0DEG = 0x00, - PIOS_MPU6000_TOP_90DEG = 0x01, - PIOS_MPU6000_TOP_180DEG = 0x02, - PIOS_MPU6000_TOP_270DEG = 0x03 + +#define PIOS_MPU6000_LOCATION_TOP 0x00 +#define PIOS_MPU6000_LOCATION_BOTTOM 0x10 + +#define PIOS_MPU6000_LOCATION_MASK 0xf0 + +enum pios_mpu6000_orientation { // clockwise rotation from board forward, when looking at sensor itself, which can be also on the bottom side + PIOS_MPU6000_TOP_0DEG = 0 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_90DEG = 1 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_180DEG = 2 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_TOP_270DEG = 3 | PIOS_MPU6000_LOCATION_TOP, + PIOS_MPU6000_BOTTOM_0DEG = 4 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_90DEG = 5 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_180DEG = 6 | PIOS_MPU6000_LOCATION_BOTTOM, + PIOS_MPU6000_BOTTOM_270DEG = 7 | PIOS_MPU6000_LOCATION_BOTTOM, }; + + struct pios_mpu6000_cfg { const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ uint8_t i2c_addr; diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index 099399f77..81cf4217f 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -379,6 +379,7 @@ enum usb_op_board_ids { USB_OP_BOARD_ID_SPRACINGF3EVO = 9, USB_OP_BOARD_ID_NUCLEOF303RE = 10, USB_OP_BOARD_ID_PIKOBLX = 11, + USB_OP_BOARD_ID_TINYFISH = 12, } __attribute__((packed)); enum usb_op_board_modes { diff --git a/flight/targets/boards/tinyfish/board-info.mk b/flight/targets/boards/tinyfish/board-info.mk new file mode 100644 index 000000000..fc6056e7d --- /dev/null +++ b/flight/targets/boards/tinyfish/board-info.mk @@ -0,0 +1,24 @@ +BOARD_TYPE := 0x10 +BOARD_REVISION := 0x06 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +CHIP := STM32F303xC + +OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg +OPENOCD_CONFIG := stm32f1x.cfg + +# Note: These must match the values in link_$(BOARD)_memory.ld +BL_BANK_BASE := 0x08000000 # Start of bootloader flash +BL_BANK_SIZE := 0x00004000 # Should include BD_INFO region + +# Some KB for settings storage (32kb) +EE_BANK_BASE := 0x08004000 # EEPROM storage area +EE_BANK_SIZE := 0x00008000 # EEPROM storage size + +FW_BANK_BASE := 0x0800C000 # Start of firmware flash +FW_BANK_SIZE := 0x00034000 # Should include FW_DESC_SIZE (208kB) + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/tinyfish/board_hw_defs.c b/flight/targets/boards/tinyfish/board_hw_defs.c new file mode 100644 index 000000000..7ce6630e2 --- /dev/null +++ b/flight/targets/boards/tinyfish/board_hw_defs.c @@ -0,0 +1,643 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, + [PIOS_LED_ALARM] = { /* or PC15 ?? */ + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, + [PIOS_LED_BUZZER] = { /* not really LED, but buzzer! (or PB2 ??) */ + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .active_low = false + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg = { + .gpios = pios_leds, + .num_gpios = NELEMENTS(pios_leds), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_led_cfg; +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* SPI Flash interface */ + +static const struct pios_spi_cfg pios_spi2_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi2_id; + +static const struct pios_spi_cfg pios_spi1_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ + }, + .use_crc = false, + .remap = GPIO_AF_5, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .slave_count = 1, + .ssel = { + { /* MPU6000 */ + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } + }, +}; + +uint32_t pios_spi1_id; + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" +#include "pios_flash_internal_priv.h" + +static const struct pios_flash_internal_cfg flash_internal_system_cfg = {}; + +static const struct flashfs_logfs_cfg flashfs_internal_cfg = { + .fs_magic = 0x99abcfef, + .total_fs_size = EE_BANK_SIZE, /* 32K bytes (16x2KB sectors) */ + .arena_size = 0x00004000, /* 64 * slot size = 16K bytes = 8 sectors */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = EE_BANK_BASE, /* start after the bootloader */ + .sector_size = 0x00000800, /* 2K bytes */ + .page_size = 0x00000800, /* 2K bytes */ +}; + +#endif /* PIOS_INCLUDE_FLASH */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_15_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_BRK_TIM15_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_16_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_UP_TIM16_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_17_cfg = { + .timer = TIM4, + .time_base_init = &tim_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include + +#define GPIO_AF_PA1_TIM2 GPIO_AF_1 +#define GPIO_AF_PA0_TIM2 GPIO_AF_1 +#define GPIO_AF_PA8_TIM1 GPIO_AF_6 +#define GPIO_AF_PA2_TIM2 GPIO_AF_1 +#define GPIO_AF_PB6_TIM4 GPIO_AF_2 +#define GPIO_AF_PB5_TIM3 GPIO_AF_2 +#define GPIO_AF_PB0_TIM3 GPIO_AF_2 +#define GPIO_AF_PB1_TIM3 GPIO_AF_2 +#define GPIO_AF_PB9_TIM4 GPIO_AF_2 +#define GPIO_AF_PB8_TIM4 GPIO_AF_2 +#define GPIO_AF_PB7_TIM4 GPIO_AF_2 +#define GPIO_AF_PB4_TIM3 GPIO_AF_2 +#define GPIO_AF_PB11_TIM2 GPIO_AF_1 +#define GPIO_AF_PA15_TIM8 GPIO_AF_2 +#define GPIO_AF_PA3_TIM15 GPIO_AF_9 +#define GPIO_AF_PA6_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM3 GPIO_AF_2 +#define GPIO_AF_PA10_TIM2 GPIO_AF_10 + +#define GPIO_AF_PA4_TIM3 GPIO_AF_2 +#define GPIO_AF_PA7_TIM17 GPIO_AF_1 +#define GPIO_AF_PB8_TIM16 GPIO_AF_1 + +#define GPIO_AF_PB9_TIM8 GPIO_AF_10 +#define GPIO_AF_PA2_TIM15 GPIO_AF_9 +#define GPIO_AF_PB10_TIM2 GPIO_AF_1 + +#define TIM1_CH1_DMA_INSTANCE 1 +#define TIM1_CH1_DMA_CHANNEL 2 + +#define TIM16_CH1_DMA_INSTANCE 2 +#define TIM16_CH1_DMA_CHANNEL 3 + +static const struct pios_tim_channel pios_tim_servoport_pins[] = { + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8), // bank 1 + TIM_SERVO_CHANNEL_CONFIG(TIM8, 3, B, 9), // bank 2 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 2, A, 3), // bank 3 + TIM_SERVO_CHANNEL_CONFIG(TIM15, 1, A, 2), // bank 3 + + // following two are here so we can have two additional PWM outputs instead of UART3 + TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11), // bank 4 (UART3 RX) + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, B, 10), // bank 4 (UART3 TX) +}; + +#if defined(PIOS_INCLUDE_USART) + +#define GPIO_AF_USART1 GPIO_AF_7 +#define GPIO_AF_USART2 GPIO_AF_7 +#define GPIO_AF_USART3 GPIO_AF_7 + + +#include "pios_usart_priv.h" +#include "pios_usart_config.h" + +static const struct pios_usart_cfg pios_usart_cfg[] = { + USART_CONFIG(USART1, B, 7, B, 6), /* RX_DEBUG, TX_??? */ + USART_CONFIG(USART2, A, 15, A, 14), /* RX_SBUS, TX_FRSKY_TELEMETRY */ + USART_CONFIG(USART3, B, 11, B, 10), +}; + + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler(void); +void RTC_WKUP_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div32, + .prescaler = 25 - 1, // 8Mhz / 32 / 16 / 25 => 625Hz + .irq = { + .init = { + .NVIC_IRQChannel = RTC_WKUP_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler(void) +{ + PIOS_RTC_irq_handler(); +} + +#endif /* if defined(PIOS_INCLUDE_RTC) */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins) - 2, +}; + +const struct pios_servo_cfg pios_servo_uart3_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_pins), +}; + + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +static const struct pios_tim_channel pios_tim_ppm = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11); + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm, + .num_channels = 1, +}; +#endif /* PIOS_INCLUDE_PPM */ + + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_usb_main_cfg; +} +#endif /* PIOS_INCLUDE_USB */ + +#if defined(PIOS_INCLUDE_WS2811) +#include "pios_ws2811_cfg.h" + +static const struct pios_ws2811_cfg pios_ws2811_cfg = PIOS_WS2811_CONFIG(TIM1, 1, A, 8); + +void DMA1_Channel2_IRQHandler() +{ + PIOS_WS2811_DMA_irq_handler(); +} + +#endif /* PIOS_INCLUDE_WS2811 */ + +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +void PIOS_ADC_DMC_irq_handler(void); +void DMA2_Channel5_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); + +struct pios_adc_cfg pios_adc_cfg = { + .adc_dev = ADC3, + .dma = { + .irq = { + .flags = (DMA2_FLAG_TC5 | DMA2_FLAG_TE5 | DMA2_FLAG_HT5), + .init = { + .NVIC_IRQChannel = DMA2_Channel5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA2_Channel5, + } + }, + .half_flag = DMA2_FLAG_HT5, + .full_flag = DMA2_FLAG_TC5, +}; + +void PIOS_ADC_DMC_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_ADC_DMA_Handler(); +} +const struct pios_adc_cfg *PIOS_BOARD_HW_DEFS_GetAdcCfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_adc_cfg; +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +/** + * Configuration for MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line13, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI15_10_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line13, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 8 for 1000 Hz + .Smpl_rate_div_no_dlp = 7, + // Clock at 1 khz, downsampled by 1 for 1000 Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_BOTTOM_90DEG, +#ifdef PIOS_INCLUDE_SPI + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, +#endif /* PIOS_INCLUDE_SPI */ + .max_downsample = 2 +}; + +const struct pios_mpu6000_cfg *PIOS_BOARD_HW_DEFS_GetMPU6000Cfg(__attribute__((unused)) uint32_t board_revision) +{ + return &pios_mpu6000_cfg; +} +#endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/targets/boards/tinyfish/bootloader/Makefile b/flight/targets/boards/tinyfish/bootloader/Makefile new file mode 100644 index 000000000..f31cf6e42 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/Makefile @@ -0,0 +1,29 @@ +# +# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + +ifndef FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk +include $(FLIGHT_ROOT_DIR)/make/boot-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for tinyFISH FC, board revision $(BOARD_REVISION)) diff --git a/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h b/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h new file mode 100644 index 000000000..bd8022938 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/inc/pios_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS modules */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG +#define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT + +#endif /* PIOS_CONFIG_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 000000000..8efcf63c2 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/inc/pios_usb_board_data.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB definitions + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 2 + +#include /* struct usb_* */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_TINYFISH, USB_OP_BOARD_MODE_BL) +#define PIOS_USB_BOARD_SN_SUFFIX "+BL" + +/* + * The bootloader uses a simplified report structure + * BL: ... + * FW: ... + * This define changes the behaviour in pios_usb_hid.c + */ +#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/tinyfish/bootloader/main.c b/flight/targets/boards/tinyfish/bootloader/main.c new file mode 100644 index 000000000..044608da5 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/main.c @@ -0,0 +1,230 @@ +/** + ****************************************************************************** + * @addtogroup BootLoader + * + * @{ + * @file main.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the LibrePilot BootLoader + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern void FLASH_Download(); +#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1) + +/* Private typedef -----------------------------------------------------------*/ +typedef void (*pFunction)(void); +/* Private define ------------------------------------------------------------*/ +/* Private macro -------------------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +pFunction Jump_To_Application; +uint32_t JumpAddress; + +/// LEDs PWM +uint32_t period1 = 5000; // 5 mS +uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS +uint32_t period2 = 5000; // 5 mS +uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS + + +//////////////////////////////////////// +uint8_t tempcount = 0; + +/* Extern variables ----------------------------------------------------------*/ +DFUStates DeviceState; +int16_t status = 0; +uint8_t JumpToApp = FALSE; +uint8_t GO_dfu = FALSE; +uint8_t USB_connected = FALSE; +uint8_t User_DFU_request = FALSE; +static uint8_t mReceive_Buffer[63]; +/* Private function prototypes -----------------------------------------------*/ +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); +uint8_t processRX(); +void jump_to_app(); + +int main() +{ + PIOS_SYS_Init(); + PIOS_Board_Init(); + PIOS_IAP_Init(); + + USB_connected = PIOS_USB_CableConnected(0); + + if (PIOS_IAP_CheckRequest() == TRUE) { + PIOS_DELAY_WaitmS(1000); + User_DFU_request = TRUE; + PIOS_IAP_ClearRequest(); + } + + GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); + + if (GO_dfu == TRUE) { + PIOS_Board_Init(); + if (User_DFU_request == TRUE) { + DeviceState = DFUidle; + } else { + DeviceState = BLidle; + } + } else { + JumpToApp = TRUE; + } + + uint32_t stopwatch = 0; + uint32_t prev_ticks = PIOS_DELAY_GetuS(); + while (TRUE) { + /* Update the stopwatch */ + uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); + prev_ticks += elapsed_ticks; + stopwatch += elapsed_ticks; + + if (JumpToApp == TRUE) { + jump_to_app(); + } + + switch (DeviceState) { + case Last_operation_Success: + case uploadingStarting: + case DFUidle: + period1 = 5000; + sweep_steps1 = 100; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case uploading: + period1 = 5000; + sweep_steps1 = 100; + period2 = 2500; + sweep_steps2 = 50; + break; + case downloading: + period1 = 2500; + sweep_steps1 = 50; + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + case BLidle: + period1 = 0; + PIOS_LED_On(PIOS_LED_HEARTBEAT); + period2 = 0; + break; + default: // error + period1 = 5000; + sweep_steps1 = 100; + period2 = 5000; + sweep_steps2 = 100; + } + + if (period1 != 0) { + if (LedPWM(period1, sweep_steps1, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } + + if (period2 != 0) { + if (LedPWM(period2, sweep_steps2, stopwatch)) { + PIOS_LED_On(PIOS_LED_HEARTBEAT); + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + } else { + PIOS_LED_Off(PIOS_LED_HEARTBEAT); + } + + if (stopwatch > 50 * 1000 * 1000) { + stopwatch = 0; + } + if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) /*|| (DeviceState == DFUidle && !USB_connected)*/)) { + JumpToApp = TRUE; + } + + processRX(); + DataDownload(start); + } +} + +void jump_to_app() +{ + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; + + // Check for the two possible irqstack locations (sram or core coupled sram) + if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { + /* Jump to user application */ + FLASH_Lock(); + RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); + RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); + RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); + + _SetCNTR(0); // clear interrupt mask + _SetISTR(0); // clear all requests + + JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); + Jump_To_Application = (pFunction)JumpAddress; + /* Initialize user application's Stack Pointer */ + __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); + Jump_To_Application(); + } else { + DeviceState = failed_jump; + JumpToApp = FALSE; + return; + } +} +uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) +{ + uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */ + uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */ + + uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */ + + if (curr_sweep & 1) { + pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */ + } + return ((count % pwm_period) > pwm_duty) ? 1 : 0; +} + +uint8_t processRX() +{ + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { + processComand(mReceive_Buffer); + } + return TRUE; +} + +int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len) +{ + return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len); +} diff --git a/flight/targets/boards/tinyfish/bootloader/pios_board.c b/flight/targets/boards/tinyfish/bootloader/pios_board.c new file mode 100644 index 000000000..fbb6b50f7 --- /dev/null +++ b/flight/targets/boards/tinyfish/bootloader/pios_board.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include + + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#include "pios_usbhook.h" + +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uint32_t pios_com_telem_usb_id; + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +static bool board_init_complete = false; +void PIOS_Board_Init(void) +{ + if (board_init_complete) { + return; + } + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(ENABLE); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { + PIOS_Assert(0); + } +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + +#endif /* PIOS_INCLUDE_USB */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar + + board_init_complete = true; +} + +void PIOS_ADC_DMA_Handler() +{} diff --git a/flight/targets/boards/tinyfish/firmware/Makefile b/flight/targets/boards/tinyfish/firmware/Makefile new file mode 100644 index 000000000..2976aa895 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/Makefile @@ -0,0 +1,117 @@ +# +# Copyright (C) 2015-2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# Copyright (C) 2012, PhoenixPilot, http://github.com/PhoenixPilot +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +##### + +ifndef FLIGHT_MAKEFILE + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk + +# C++ support +USE_CXX = YES + +# ARM DSP library +USE_DSP_LIB ?= NO + +DEBUG = NO + +# List of mandatory modules to include +MODULES += Sensors +MODULES += StateEstimation +MODULES += Stabilization +MODULES += ManualControl +MODULES += Receiver +MODULES += Actuator +MODULES += FirmwareIAP +#MODULES += PathPlanner +#MODULES += PathFollower +#MODULES += Osd/osdoutout +#MODULES += Logging +MODULES += Telemetry +MODULES += Notify + +OPTMODULES += Airspeed +OPTMODULES += AutoTune +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += CameraStab +OPTMODULES += CameraControl +OPTMODULES += Battery +OPTMODULES += ComUsbBridge +OPTMODULES += UAVOMSPBridge +OPTMODULES += UAVOMavlinkBridge + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + +# Some diagnostics +CDEFS += -DDIAG_TASKS + +# Misc options +CFLAGS += -ffast-math + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + CPPSRC += $(OPSYSTEM)/main.cpp + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + + #ifeq ($(DEBUG), YES) + SRC += $(OPSYSTEM)/dcc_stdio.c + SRC += $(OPSYSTEM)/cm3_fault_handlers.c + #endif + + ## Misc library functions + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(FLIGHTLIB)/paths.c + SRC += $(FLIGHTLIB)/plans.c + SRC += $(FLIGHTLIB)/WorldMagModel.c + SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/auxmagsupport.c + SRC += $(FLIGHTLIB)/lednotification.c + + ## UAVObjects + include ./UAVObjects.inc + SRC += $(UAVOBJSRC) +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +# Optional component libraries +#include $(FLIGHTLIB)/rscode/library.mk +#include $(FLIGHTLIB)/PyMite/pymite.mk + +include $(FLIGHT_ROOT_DIR)/make/apps-defs.mk +include $(FLIGHT_ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/tinyfish/firmware/UAVObjects.inc b/flight/targets/boards/tinyfish/firmware/UAVObjects.inc new file mode 100644 index 000000000..e68307fa5 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/UAVObjects.inc @@ -0,0 +1,136 @@ +# +# Copyright (C) 2016, The LibrePilot Project, http://www.librepilot.org +# Copyright (C) 2009-2013, The OpenPilot Team, http://www.openpilot.org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. +# +# You should have received a copy of the GNU General Public License along +# with this program; if not, write to the Free Software Foundation, Inc., +# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +# + +# These are the UAVObjects supposed to be build as part of the OpenPilot target +# (all architectures) +UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus +UAVOBJSRCFILENAMES += statusvtolland +UAVOBJSRCFILENAMES += statusvtolautotakeoff +UAVOBJSRCFILENAMES += vtolselftuningstats +UAVOBJSRCFILENAMES += accelgyrosettings +UAVOBJSRCFILENAMES += accessorydesired +UAVOBJSRCFILENAMES += actuatorcommand +UAVOBJSRCFILENAMES += actuatordesired +UAVOBJSRCFILENAMES += actuatorsettings +UAVOBJSRCFILENAMES += attitudesettings +UAVOBJSRCFILENAMES += attitudestate +UAVOBJSRCFILENAMES += gyrostate +UAVOBJSRCFILENAMES += gyrosensor +UAVOBJSRCFILENAMES += accelstate +UAVOBJSRCFILENAMES += accelsensor +UAVOBJSRCFILENAMES += magsensor +UAVOBJSRCFILENAMES += auxmagsensor +UAVOBJSRCFILENAMES += auxmagsettings +UAVOBJSRCFILENAMES += magstate +UAVOBJSRCFILENAMES += barosensor +UAVOBJSRCFILENAMES += airspeedsensor +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedstate +UAVOBJSRCFILENAMES += debuglogsettings +UAVOBJSRCFILENAMES += debuglogcontrol +UAVOBJSRCFILENAMES += debuglogstatus +UAVOBJSRCFILENAMES += debuglogentry +UAVOBJSRCFILENAMES += flightbatterysettings +UAVOBJSRCFILENAMES += firmwareiapobj +UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplancontrol +UAVOBJSRCFILENAMES += flightplansettings +UAVOBJSRCFILENAMES += flightplanstatus +UAVOBJSRCFILENAMES += flighttelemetrystats +UAVOBJSRCFILENAMES += gcstelemetrystats +UAVOBJSRCFILENAMES += gcsreceiver +UAVOBJSRCFILENAMES += gpspositionsensor +UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocitysensor +UAVOBJSRCFILENAMES += gpssettings +UAVOBJSRCFILENAMES += gpsextendedstatus +UAVOBJSRCFILENAMES += fixedwingpathfollowersettings +UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus +UAVOBJSRCFILENAMES += vtolpathfollowersettings +UAVOBJSRCFILENAMES += groundpathfollowersettings +UAVOBJSRCFILENAMES += homelocation +UAVOBJSRCFILENAMES += i2cstats +UAVOBJSRCFILENAMES += manualcontrolcommand +UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings +UAVOBJSRCFILENAMES += mixersettings +UAVOBJSRCFILENAMES += mixerstatus +UAVOBJSRCFILENAMES += nedaccel +UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver +UAVOBJSRCFILENAMES += overosyncstats +UAVOBJSRCFILENAMES += overosyncsettings +UAVOBJSRCFILENAMES += pathaction +UAVOBJSRCFILENAMES += pathdesired +UAVOBJSRCFILENAMES += pathplan +UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary +UAVOBJSRCFILENAMES += positionstate +UAVOBJSRCFILENAMES += ratedesired +UAVOBJSRCFILENAMES += ekfconfiguration +UAVOBJSRCFILENAMES += ekfstatevariance +UAVOBJSRCFILENAMES += revocalibration +UAVOBJSRCFILENAMES += revosettings +UAVOBJSRCFILENAMES += sonaraltitude +UAVOBJSRCFILENAMES += stabilizationdesired +UAVOBJSRCFILENAMES += stabilizationsettings +UAVOBJSRCFILENAMES += stabilizationsettingsbank1 +UAVOBJSRCFILENAMES += stabilizationsettingsbank2 +UAVOBJSRCFILENAMES += stabilizationsettingsbank3 +UAVOBJSRCFILENAMES += stabilizationstatus +UAVOBJSRCFILENAMES += stabilizationbank +UAVOBJSRCFILENAMES += systemalarms +UAVOBJSRCFILENAMES += systemsettings +UAVOBJSRCFILENAMES += systemstats +UAVOBJSRCFILENAMES += taskinfo +UAVOBJSRCFILENAMES += callbackinfo +UAVOBJSRCFILENAMES += velocitystate +UAVOBJSRCFILENAMES += velocitydesired +UAVOBJSRCFILENAMES += watchdogstatus +UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += hwtinyfishsettings +UAVOBJSRCFILENAMES += receiveractivity +UAVOBJSRCFILENAMES += receiverstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += altitudefiltersettings +UAVOBJSRCFILENAMES += altitudeholdstatus +UAVOBJSRCFILENAMES += waypoint +UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings +UAVOBJSRCFILENAMES += mpugyroaccelsettings +UAVOBJSRCFILENAMES += txpidsettings +UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += takeofflocation +UAVOBJSRCFILENAMES += perfcounter +UAVOBJSRCFILENAMES += systemidentsettings +UAVOBJSRCFILENAMES += systemidentstate +UAVOBJSRCFILENAMES += cameracontrolsettings +UAVOBJSRCFILENAMES += cameracontrolactivity + +UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(FLIGHT_UAVOBJ_DIR)/$(UAVOBJSRCFILE).c ) +UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/targets/boards/tinyfish/firmware/cm3_fault_handlers.c b/flight/targets/boards/tinyfish/firmware/cm3_fault_handlers.c new file mode 100644 index 000000000..cf9142857 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/cm3_fault_handlers.c @@ -0,0 +1,93 @@ +/* + * cm3_fault_handlers.c + * + * Created on: Apr 24, 2011 + * Author: msmith + */ + +#include +#include "inc/dcc_stdio.h" +#ifdef STM32F4XX +# include +#endif +#ifdef STM32F3 +# include +#endif +#ifdef STM32F2XX +# include +#endif + +#define FAULT_TRAMPOLINE(_vec) \ + __attribute__((naked, no_instrument_function)) \ + void \ + _vec##_Handler(void) \ + { \ + __asm(".syntax unified\n" \ + "MOVS R0, #4 \n" \ + "MOV R1, LR \n" \ + "TST R0, R1 \n" \ + "BEQ 1f \n" \ + "MRS R0, PSP \n" \ + "B " #_vec "_Handler2 \n" \ + "1: \n" \ + "MRS R0, MSP \n" \ + "B " #_vec "_Handler2 \n" \ + ".syntax divided\n"); \ + } \ + struct hack + +struct cm3_frame { + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; +}; + +FAULT_TRAMPOLINE(HardFault); +FAULT_TRAMPOLINE(BusFault); +FAULT_TRAMPOLINE(UsageFault); + +/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */ +#define SCB_REG(_reg) (*(uint32_t *)&(SCB->_reg)) + +void HardFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nHARD FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(HFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} + +void BusFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nBUS FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(BFAR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} + +void UsageFault_Handler2(struct cm3_frame *frame) +{ + dbg_write_str("\nUSAGE FAULT"); + dbg_write_hex32(frame->pc); + dbg_write_char('\n'); + dbg_write_hex32(SCB_REG(CFSR)); + dbg_write_char('\n'); + for (;;) { + ; + } +} diff --git a/flight/targets/boards/tinyfish/firmware/dcc_stdio.c b/flight/targets/boards/tinyfish/firmware/dcc_stdio.c new file mode 100644 index 000000000..1a522e9a0 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/dcc_stdio.c @@ -0,0 +1,149 @@ +/*************************************************************************** +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* Copyright (C) 2008 by Frederik Kriewtz * +* frederik@kriewitz.eu * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ + +#include "inc/dcc_stdio.h" + +#define TARGET_REQ_TRACEMSG 0x00 +#define TARGET_REQ_DEBUGMSG_ASCII 0x01 +#define TARGET_REQ_DEBUGMSG_HEXMSG(size) (0x01 | ((size & 0xff) << 8)) +#define TARGET_REQ_DEBUGCHAR 0x02 + +/* we use the cortex_m3 DCRDR reg to simulate a arm7_9 dcc channel + * DCRDR[7:0] is used by target for status + * DCRDR[15:8] is used by target for write buffer + * DCRDR[23:16] is used for by host for status + * DCRDR[31:24] is used for by host for write buffer */ + +#define NVIC_DBG_DATA_R (*((volatile unsigned short *)0xE000EDF8)) + +#define BUSY 1 + +void dbg_write(unsigned long dcc_data) +{ + int len = 4; + + while (len--) { + /* wait for data ready */ + while (NVIC_DBG_DATA_R & BUSY) { + ; + } + + /* write our data and set write flag - tell host there is data*/ + NVIC_DBG_DATA_R = (unsigned short)(((dcc_data & 0xff) << 8) | BUSY); + dcc_data >>= 8; + } +} + +void dbg_trace_point(unsigned long number) +{ + dbg_write(TARGET_REQ_TRACEMSG | (number << 8)); +} + +void dbg_write_u32(const unsigned long *val, long len) +{ + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16)); + + while (len > 0) { + dbg_write(*val); + + val++; + len--; + } +} + +void dbg_write_u16(const unsigned short *val, long len) +{ + unsigned long dcc_data; + + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16)); + + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 16 : 0x0000); + + dbg_write(dcc_data); + + val += 2; + len -= 2; + } +} + +void dbg_write_u8(const unsigned char *val, long len) +{ + unsigned long dcc_data; + + dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16)); + + while (len > 0) { + dcc_data = val[0] + | ((len > 1) ? val[1] << 8 : 0x00) + | ((len > 2) ? val[2] << 16 : 0x00) + | ((len > 3) ? val[3] << 24 : 0x00); + + dbg_write(dcc_data); + + val += 4; + len -= 4; + } +} + +void dbg_write_str(const char *msg) +{ + long len; + unsigned long dcc_data; + + for (len = 0; msg[len] && (len < 65536); len++) { + ; + } + + dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16)); + + while (len > 0) { + dcc_data = msg[0] + | ((len > 1) ? msg[1] << 8 : 0x00) + | ((len > 2) ? msg[2] << 16 : 0x00) + | ((len > 3) ? msg[3] << 24 : 0x00); + dbg_write(dcc_data); + + msg += 4; + len -= 4; + } +} + +void dbg_write_char(char msg) +{ + dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16)); +} + +void dbg_write_hex32(const unsigned long val) +{ + static const char hextab[] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' + }; + + for (int shift = 28; shift >= 0; shift -= 4) { + dbg_write_char(hextab[(val >> shift) & 0xf]); + } +} diff --git a/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 000000000..5fea013d7 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/FreeRTOSConfig.h @@ -0,0 +1,99 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- +* Application specific definitions. +* +* These definitions should be adjusted for your particular hardware and +* application requirements. +* +* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE +* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE. +* +* See http://www.freertos.org/a00110.html. +*----------------------------------------------------------*/ + +/** + * @addtogroup PIOS PIOS + * @{ + * @addtogroup FreeRTOS FreeRTOS + * @{ + */ + +/* Notes: We use 5 task priorities */ + +#define configUSE_PREEMPTION 1 +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 +#define configCPU_CLOCK_HZ ((unsigned long)72000000) +#define configTICK_RATE_HZ ((portTickType)1000) +#define configMAX_PRIORITIES ((unsigned portBASE_TYPE)5) +#define configMINIMAL_STACK_SIZE ((unsigned short)512) +#define configTOTAL_HEAP_SIZE ((size_t)(53 * 256)) +#define configMAX_TASK_NAME_LEN (6) +#define configUSE_TRACE_FACILITY 0 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configUSE_ALTERNATIVE_API 0 +#define configQUEUE_REGISTRY_SIZE 0 + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES (2) + +/* Set the following definitions to 1 to include the API function, or zero + to exclude the API function. */ + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 0 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 + +/* This is the raw value as per the Cortex-M3 NVIC. Values can be 255 + (lowest) to 1 (highest maskable) to 0 (highest non-maskable). */ +#define configKERNEL_INTERRUPT_PRIORITY 15 << 4 /* equivalent to NVIC priority 15 */ + #define configMAX_SYSCALL_INTERRUPT_PRIORITY 3 << 4 /* equivalent to NVIC priority 3 */ + +/* This is the value being used as per the ST library which permits 16 + priority values, 0 to 15. This must correspond to the + configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest + NVIC value of 255. */ +#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 + +#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) +#define CHECK_IRQ_STACK +#endif + +/* Enable run time stats collection */ +#define configGENERATE_RUN_TIME_STATS 1 +#define INCLUDE_uxTaskGetRunTime 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() \ + do { \ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ \ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ \ + } \ + while (0) +#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004) /* DWT_CYCCNT */ + +#ifdef DIAG_TASKS +#define configCHECK_FOR_STACK_OVERFLOW 2 +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif + +/** + * @} + */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/dcc_stdio.h b/flight/targets/boards/tinyfish/firmware/inc/dcc_stdio.h new file mode 100644 index 000000000..3d7354918 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/dcc_stdio.h @@ -0,0 +1,36 @@ +/*************************************************************************** +* Copyright (C) 2008 by Dominic Rath * +* Dominic.Rath@gmx.de * +* Copyright (C) 2008 by Spencer Oliver * +* spen@spen-soft.co.uk * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License for more details. * +* * +* You should have received a copy of the GNU General Public License * +* along with this program; if not, write to the * +* Free Software Foundation, Inc., * +* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * +***************************************************************************/ + +#ifndef DCC_STDIO_H +#define DCC_STDIO_H + +void dbg_trace_point(unsigned long number); + +void dbg_write_u32(const unsigned long *val, long len); +void dbg_write_u16(const unsigned short *val, long len); +void dbg_write_u8(const unsigned char *val, long len); + +void dbg_write_str(const char *msg); +void dbg_write_char(char msg); +void dbg_write_hex32(const unsigned long val); + +#endif /* DCC_STDIO_H */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/openpilot.h b/flight/targets/boards/tinyfish/firmware/inc/openpilot.h new file mode 100644 index 000000000..5576baca6 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/openpilot.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file openpilot.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main OpenPilot header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef OPENPILOT_H +#define OPENPILOT_H + +/* PIOS Includes */ +#include + +/* OpenPilot Libraries */ +#include +#include +#include +#include + +#include "alarms.h" +#include + +/* Global Functions */ +void OpenPilotInit(void); + +#endif /* OPENPILOT_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/pios_config.h b/flight/targets/boards/tinyfish/firmware/inc/pios_config.h new file mode 100644 index 000000000..330d13067 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/pios_config.h @@ -0,0 +1,195 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author LibrePilot, https://bitbucket.org/librepilot, Copyright (C) 2015 + * @brief PiOS configuration header, the compile time config file for the PIOS. + * Defines which PiOS libraries and features are included in the firmware. + * @see The GNU Public License (GPL) Version 3 + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_CONFIG_H +#define PIOS_CONFIG_H + +/* + * Below is a complete list of PIOS configurable options. + * Please do not remove or rearrange them. Only comment out + * unused options in the list. See main pios.h header for more + * details. + */ + +/* #define PIOS_INCLUDE_DEBUG_CONSOLE */ +/* #define DEBUG_LEVEL 0 */ +/* #define PIOS_ENABLE_DEBUG_PINS */ + +/* PIOS FreeRTOS support */ +#define PIOS_INCLUDE_FREERTOS + + +/* PIOS CallbackScheduler support */ +#define PIOS_INCLUDE_CALLBACKSCHEDULER + +/* PIOS bootloader helper */ +#define PIOS_INCLUDE_BL_HELPER +/* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ + +/* PIOS system functions */ +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_INITCALL +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_TASK_MONITOR +// #define PIOS_INCLUDE_INSTRUMENTATION +#define PIOS_INSTRUMENTATION_MAX_COUNTERS 5 + +/* PIOS hardware peripherals */ +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_TIM +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_ADC +// #define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_WDG + +/* PIOS USB functions */ +#define PIOS_INCLUDE_USB +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC +/* #define PIOS_INCLUDE_USB_RCTX */ + +/* PIOS sensor interfaces */ +/* #define PIOS_INCLUDE_ADXL345 */ +/* #define PIOS_INCLUDE_BMA180 */ +/* #define PIOS_INCLUDE_L3GD20 */ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL +/* #define PIOS_INCLUDE_HMC5843 */ +/* #define PIOS_INCLUDE_HMC5X83 */ +/* #define PIOS_HMC5883_HAS_GPIOS */ +/* #define PIOS_INCLUDE_MPU9250 */ +/* #define PIOS_MPU9250_ACCEL */ +/* #define PIOS_MPU9250_MAG */ +/* #define PIOS_INCLUDE_BMP280 */ +/* #define PIOS_INCLUDE_BMP085 */ +/* #define PIOS_INCLUDE_MS5611 */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +#define PIOS_INCLUDE_WS2811 + +/* PIOS receiver drivers */ +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM_FLEXI +#define PIOS_INCLUDE_DSM +#define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS +#define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT +#define PIOS_INCLUDE_IBUS +/* #define PIOS_INCLUDE_GCSRCVR */ +/* #define PIOS_INCLUDE_OPLINKRCVR */ + +/* PIOS abstract receiver interface */ +#define PIOS_INCLUDE_RCVR + +/* PIOS common peripherals */ +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_SERVO +/* #define PIOS_INCLUDE_I2C_ESC */ +/* #define PIOS_INCLUDE_OVERO */ +/* #define PIOS_OVERO_SPI */ +/* #define PIOS_INCLUDE_SDCARD */ +/* #define LOG_FILENAME "startup.log" */ +#define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_FLASH_LOGFS_SETTINGS +#define FLASH_FREERTOS +/* #define PIOS_INCLUDE_FLASH_EEPROM */ +#define PIOS_INCLUDE_FLASH_INTERNAL + +/* PIOS radio modules */ +/* #define PIOS_INCLUDE_RFM22B */ +/* #define PIOS_INCLUDE_RFM22B_COM */ +/* #define PIOS_INCLUDE_PPM_OUT */ +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ + +/* PIOS misc peripherals */ +/* #define PIOS_INCLUDE_VIDEO */ +/* #define PIOS_INCLUDE_WAVE */ +/* #define PIOS_INCLUDE_UDP */ + +/* PIOS abstract comms interface with options */ +#define PIOS_INCLUDE_COM +/* #define PIOS_INCLUDE_COM_MSG */ +#define PIOS_INCLUDE_TELEMETRY_RF +/* #define PIOS_INCLUDE_COM_TELEM */ +/* #define PIOS_INCLUDE_COM_FLEXI */ +/* #define PIOS_INCLUDE_COM_AUX */ +/* #define PIOS_TELEM_PRIORITY_QUEUE */ +#define PIOS_INCLUDE_GPS +/* #define PIOS_GPS_MINIMAL */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER +#define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER +#define PIOS_GPS_SETS_HOMELOCATION + +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION +/* #define PIOS_EXCLUDE_ADVANCED_FEATURES */ +/* Performance counters */ +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 40 +#define IRQSTACK_LIMIT_WARNING 100 +#define IRQSTACK_LIMIT_CRITICAL 60 +#define CPULOAD_LIMIT_WARNING 85 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 800 +#define PIOS_MANUAL_STACK_SIZE 935 +#define PIOS_RECEIVER_STACK_SIZE 840 +#define PIOS_SYSTEM_STACK_SIZE 1536 +/* #define PIOS_STABILIZATION_STACK_SIZE 400 */ + +#define PIOS_TELEM_STACK_SIZE 800 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 256 + +/* This can't be too high to stop eventdispatcher thread overflowing */ +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + +/* Revolution series */ +#define REVOLUTION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h new file mode 100644 index 000000000..36342d32d --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/inc/pios_usb_board_data.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB definitions + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_USB_BOARD_DATA_H +#define PIOS_USB_BOARD_DATA_H + +// Note : changing below length will require changes to the USB buffer setup +#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64 +#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32 +#define PIOS_USB_BOARD_HID_DATA_LENGTH 64 + +#define PIOS_USB_BOARD_EP_NUM 4 + +#include /* USB_* macros */ + +#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_LIBREPILOT +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_TINYFISH, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/tinyfish/firmware/main.cpp b/flight/targets/boards/tinyfish/firmware/main.cpp new file mode 100644 index 000000000..d1ebd3625 --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/main.cpp @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup LibrePilotSystem LibrePilot System + * @brief These files are the core system files for LibrePilot. + * They are the ground layer just above PiOS. In practice, LibrePilot actually starts + * in the main() function of main.cpp + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @brief This is where the LP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file main.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2015 + * @brief Sets up and runs main tasks. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +extern "C" { +#include "inc/openpilot.h" +#include +/* Task Priorities */ + +/* Global Variables */ +extern void Stack_Change(void); +} /* extern "C" */ + +/** + * OpenPilot Main function: + * + * Initialize PiOS
+ * Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+ * Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) + * If something goes wrong, blink LED1 and LED2 every 100ms + * + */ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + vPortInitialiseBlocks(); + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + + SystemModStart(); + + /* Start the FreeRTOS scheduler, which should never return. + * + * NOTE: OpenPilot runs an operating system (FreeRTOS), which constantly calls + * (schedules) function files (modules). These functions never return from their + * while loops, which explains why each module has a while(1){} segment. Thus, + * the OpenPilot software actually starts at the vTaskStartScheduler() function, + * even though this is somewhat obscure. + * + * In addition, there are many main() functions in the OpenPilot firmware source tree + * This is because each main() refers to a separate hardware platform. Of course, + * C only allows one main(), so only the relevant main() function is compiled when + * making a specific firmware. + * + */ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } + + return 0; +} + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/tinyfish/firmware/pios_board.c b/flight/targets/boards/tinyfish/firmware/pios_board.c new file mode 100644 index 000000000..f6453aa4d --- /dev/null +++ b/flight/targets/boards/tinyfish/firmware/pios_board.c @@ -0,0 +1,260 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup LibrePilotSystem LibrePilot System + * @{ + * @addtogroup LibrePilotCore LibrePilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the tinyFISH board. + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif + +#include +#include + +/* + * Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "../board_hw_defs.c" + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; + +#ifdef PIOS_INCLUDE_WS2811 +uint32_t pios_ws2811_id; +#endif + + +static HwTinyFISHSettingsData boardHwSettings; + +static void hwTinyFISHSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + HwTinyFISHSettingsData currentBoardHwSettings; + + HwTinyFISHSettingsGet(¤tBoardHwSettings); + + if (memcmp(¤tBoardHwSettings, &boardHwSettings, sizeof(HwTinyFISHSettingsData)) != 0) { + ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); + } +} + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ + +void PIOS_Board_Init(void) +{ +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(pios_board_info_blob.board_rev); + PIOS_DEBUG_Assert(led_cfg); + PIOS_LED_Init(led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#ifdef PIOS_INCLUDE_INSTRUMENTATION + PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS); +#endif + +#if defined(PIOS_INCLUDE_SPI) + /* Set up the SPI interface to the mpu6000 */ + + if (PIOS_SPI_Init(&pios_spi1_id, &pios_spi1_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_SPI_Init(&pios_spi2_id, &pios_spi2_cfg)) { + PIOS_DEBUG_Assert(0); + } +#endif + +#if defined(PIOS_INCLUDE_FLASH) + /* Connect flash to the appropriate interface and configure it */ + uintptr_t flash_id; + + // initialize the internal settings storage flash + if (PIOS_Flash_Internal_Init(&flash_id, &flash_internal_system_cfg)) { + PIOS_DEBUG_Assert(0); + } + + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + + /* init SPI flash here */ + +#endif /* if defined(PIOS_INCLUDE_FLASH) */ + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + + HwSettingsInitialize(); + HwTinyFISHSettingsInitialize(); + +#ifndef ERASE_FLASH +#ifdef PIOS_INCLUDE_WDG + /* Initialize watchdog as early as possible to catch faults during init */ + PIOS_WDG_Init(); +#endif +#endif + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Check for repeated boot failures */ + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + HwTinyFISHSettingsSetDefaults(HwTinyFISHSettingsHandle(), 0); + + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); +// PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + PIOS_TIM_InitClock(&tim_15_cfg); +// PIOS_TIM_InitClock(&tim_16_cfg); +// PIOS_TIM_InitClock(&tim_17_cfg); + +#if defined(PIOS_INCLUDE_USB) + PIOS_BOARD_IO_Configure_USB(); +#endif + + HwTinyFISHSettingsConnectCallback(hwTinyFISHSettingsUpdatedCb); + + HwTinyFISHSettingsGet(&boardHwSettings); + + static const PIOS_BOARD_IO_UART_Function uart_function_map[] = { + [HWTINYFISHSETTINGS_UARTPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWTINYFISHSETTINGS_UARTPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWTINYFISHSETTINGS_UARTPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWTINYFISHSETTINGS_UARTPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, // single DSM instance? ok. + [HWTINYFISHSETTINGS_UARTPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWTINYFISHSETTINGS_UARTPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWTINYFISHSETTINGS_UARTPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWTINYFISHSETTINGS_UARTPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWTINYFISHSETTINGS_UARTPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWTINYFISHSETTINGS_UARTPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWTINYFISHSETTINGS_UARTPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWTINYFISHSETTINGS_UARTPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWTINYFISHSETTINGS_UARTPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, + }; + + for (unsigned int i = 0; i < HWTINYFISHSETTINGS_UARTPORT_NUMELEM; ++i) { + if (boardHwSettings.UARTPort[i] < NELEMENTS(uart_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_cfg[i], uart_function_map[boardHwSettings.UARTPort[i]]); + } + } + + const struct pios_servo_cfg *servo_cfg = (boardHwSettings.UARTPort[2] == HWTINYFISHSETTINGS_UARTPORT_OUTPUTS) ? &pios_servo_uart3_cfg : &pios_servo_cfg; + + if(boardHwSettings.UARTPort[2] == HWTINYFISHSETTINGS_UARTPORT_PPM) { + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); + } + +#ifdef PIOS_INCLUDE_PPM + PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_cfg); +#endif + +#ifndef PIOS_ENABLE_DEBUG_PINS + PIOS_Servo_Init(servo_cfg); +#else + PIOS_DEBUG_Init(&pios_servo_cfg.channels, pios_servo_cfg.num_channels); +#endif + + switch (boardHwSettings.LEDPort) { + case HWTINYFISHSETTINGS_LEDPORT_WS2811: +#if defined(PIOS_INCLUDE_WS2811) + PIOS_WS2811_Init(&pios_ws2811_id, &pios_ws2811_cfg); +#endif + break; + default: + break; + } + + if (boardHwSettings.BuzzerPort == HWTINYFISHSETTINGS_BUZZERPORT_ENABLED) { + // enable buzzer somehow + } + + PIOS_BOARD_Sensors_Configure(); + + PIOS_LED_On(PIOS_LED_HEARTBEAT); + + /* Make sure we have at least one telemetry link configured or else fail initialization */ +// PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); +} + + +/** + * @} + */ diff --git a/flight/targets/boards/tinyfish/pios_board.h b/flight/targets/boards/tinyfish/pios_board.h new file mode 100644 index 000000000..9030b1215 --- /dev/null +++ b/flight/targets/boards/tinyfish/pios_board.h @@ -0,0 +1,212 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * + * @brief Defines board hardware for the tinyFISH board + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_BOARD_H +#define PIOS_BOARD_H + +// ------------------------ +// BOOTLOADER_SETTINGS +// ------------------------ +#define BOARD_READABLE true +#define BOARD_WRITABLE true +#define MAX_DEL_RETRYS 3 + +// ------------------------ +// WATCHDOG_SETTINGS +// ------------------------ +#define PIOS_WATCHDOG_TIMEOUT 250 +#define PIOS_WDG_REGISTER RTC_BKP_DR4 +#define PIOS_WDG_ACTUATOR 0x0001 +#define PIOS_WDG_STABILIZATION 0x0002 +#define PIOS_WDG_ATTITUDE 0x0004 +#define PIOS_WDG_MANUAL 0x0008 +#define PIOS_WDG_AUTOTUNE 0x0010 +#define PIOS_WDG_SENSORS 0x0020 + + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 +#define PIOS_LED_BUZZER 2 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 + +// ------------------------- +// Interrupt Priorities +// ------------------------- +#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS +#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS +#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... +#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... +// ------------------------ +// PIOS_I2C +// See also pios_board.c +// ------------------------ +#define PIOS_I2C_MAX_DEVS 0 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 +extern uint32_t pios_spi1_id; +#define PIOS_SPI_MPU6000_ADAPTER (pios_spi1_id) + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 3 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + + +#ifdef PIOS_INCLUDE_WS2811 +extern uint32_t pios_ws2811_id; +#define PIOS_WS2811_DEVICE (pios_ws2811_id) +#endif + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Current sensor +// PIOS_ADC_PinGet(1) = Battery voltage +// ------------------------- +#define PIOS_DMA_PIN_CONFIG \ + { \ + { GPIOB, GPIO_Pin_0, ADC_Channel_12, false }, /* ADC_2 / Current */ \ + { GPIOB, GPIO_Pin_1, ADC_Channel_1, false }, /* ADC_1 / Voltage */ \ + } + +/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */ +/* which is annoying because this then determines the rate at which we generate buffer turnover events */ +/* the objective here is to get enough buffer space to support 100Hz averaging rate */ +#define PIOS_ADC_NUM_CHANNELS 2 +#define PIOS_ADC_MAX_OVERSAMPLING 32 + +// #define PIOS_ADC_USE_TEMP_SENSOR +// #define PIOS_ADC_TEMPERATURE_PIN 4 + +// ------------------------ +// PIOS_RCVR +// See also pios_board.c +// ------------------------ +#define PIOS_RCVR_MAX_DEVS 3 +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_GCSRCVR_TIMEOUT_MS 100 + +// ------------------------- +// Receiver PPM input +// ------------------------- +#define PIOS_PPM_MAX_DEVS 1 +#define PIOS_PPM_NUM_INPUTS 16 + +// ------------------------- +// Receiver PWM input +// ------------------------- +#define PIOS_PWM_MAX_DEVS 1 +#define PIOS_PWM_NUM_INPUTS 6 + +// ------------------------- +// Receiver DSM input +// ------------------------- +#define PIOS_DSM_MAX_DEVS 2 +#define PIOS_DSM_NUM_INPUTS 12 + +// ------------------------- +// Receiver S.Bus input +// ------------------------- +#define PIOS_SBUS_MAX_DEVS 1 +#define PIOS_SBUS_NUM_INPUTS (16 + 2) + +// ------------------------- +// Receiver HOTT input +// ------------------------- +#define PIOS_HOTT_MAX_DEVS 1 +#define PIOS_HOTT_NUM_INPUTS 32 + +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 1 +#define PIOS_EXBUS_NUM_INPUTS 16 + +// ------------------------- +// Receiver Multiplex SRXL input +// ------------------------- +#define PIOS_SRXL_MAX_DEVS 1 +#define PIOS_SRXL_NUM_INPUTS 16 + +// ------------------------- +// Receiver FlySky IBus input +// ------------------------- +#define PIOS_IBUS_MAX_DEVS 1 +#define PIOS_IBUS_NUM_INPUTS 10 + +// ------------------------- +// Servo outputs +// ------------------------- +#define PIOS_SERVO_UPDATE_HZ 50 +#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +#define PIOS_SERVO_BANKS 6 + +// -------------------------- +// Timer controller settings +// -------------------------- +#define PIOS_TIM_MAX_DEVS 3 + +// ------------------------- +// GPIO +// ------------------------- +#define PIOS_GPIO_PORTS {} +#define PIOS_GPIO_PINS {} +#define PIOS_GPIO_CLKS {} +#define PIOS_GPIO_NUM 0 + +// ------------------------- +// USB +// ------------------------- +#define PIOS_USB_HID_MAX_DEVS 1 + +#define PIOS_USB_ENABLED 1 +#define PIOS_USB_MAX_DEVS 1 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/tinyfish/pios_usb_board_data.c b/flight/targets/boards/tinyfish/pios_usb_board_data.c new file mode 100644 index 000000000..be0bb584e --- /dev/null +++ b/flight/targets/boards/tinyfish/pios_usb_board_data.c @@ -0,0 +1,98 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD Board specific USB definitions + * @brief Board specific USB definitions + * @{ + * + * @file pios_usb_board_data.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Board specific USB definitions + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "inc/pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include /* PIOS_SYS_SerialNumberGet */ +#include /* PIOS_USBHOOK_* */ +#include /* PIOS_USB_UTIL_AsciiToUtf8 */ + +static const uint8_t usb_product_id[18] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 't', 0, + 'i', 0, + 'n', 0, + 'y', 0, + 'F', 0, + 'I', 0, + 'S', 0, + 'H', 0, +}; + +static uint8_t usb_serial_number[2 + PIOS_SYS_SERIAL_NUM_ASCII_LEN * 2 + (sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1) * 2] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_US), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[PIOS_SYS_SERIAL_NUM_ASCII_LEN + 1]; + + PIOS_SYS_SerialNumberGet((char *)sn); + + /* Concatenate the device serial number and the appropriate suffix ("+BL" or "+FW") into the USB serial number */ + uint8_t *utf8 = &(usb_serial_number[2]); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, sn, PIOS_SYS_SERIAL_NUM_ASCII_LEN); + utf8 = PIOS_USB_UTIL_AsciiToUtf8(utf8, (uint8_t *)PIOS_USB_BOARD_SN_SUFFIX, sizeof(PIOS_USB_BOARD_SN_SUFFIX) - 1); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/ground/gcs/src/plugins/config/configgadget.qrc b/ground/gcs/src/plugins/config/configgadget.qrc index 918a0a160..bba3973cf 100644 --- a/ground/gcs/src/plugins/config/configgadget.qrc +++ b/ground/gcs/src/plugins/config/configgadget.qrc @@ -59,5 +59,6 @@ images/cc3d_top.png images/sparky2_top.png images/spracingf3_top.png + images/tinyfish_top.png diff --git a/ground/gcs/src/plugins/config/configgadgetwidget.cpp b/ground/gcs/src/plugins/config/configgadgetwidget.cpp index 449eb56fe..03ae0cd23 100644 --- a/ground/gcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/gcs/src/plugins/config/configgadgetwidget.cpp @@ -254,6 +254,8 @@ void ConfigGadgetWidget::onAutopilotConnect() // widget = new ConfigSPRacingF3EVOHWWidget(this); } else if (board == 0x1005) { // widget = new ConfigPikoBLXHWWidget(this); + } else if (board == 0x1006) { + // widget = new ConfigTinyFISHHWWidget(this); } if (widget) { widget->bind(); diff --git a/ground/gcs/src/plugins/config/configoutputwidget.cpp b/ground/gcs/src/plugins/config/configoutputwidget.cpp index 7b3e40587..644187f35 100644 --- a/ground/gcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/gcs/src/plugins/config/configoutputwidget.cpp @@ -384,6 +384,9 @@ void ConfigOutputWidget::refreshWidgetsValuesImpl(UAVObject *obj) } else if (board == 0x1005) { // PikoBLX bankLabels << "1 (1-4)" << "2 (5-6)" << "3 (7)" << "4 (8)"; channelBanks << 1 << 1 << 1 << 1 << 2 << 2 << 3 << 4; + } else if (board == 0x1006) { // tinyFISH + bankLabels << "1 (1)" << "2 (2)" << "3 (3-4)" << "4 (5-6)"; + channelBanks << 1 << 2 << 3 << 3 << 4 << 4; } } diff --git a/ground/gcs/src/plugins/config/images/tinyfish_top.png b/ground/gcs/src/plugins/config/images/tinyfish_top.png new file mode 100644 index 000000000..1f30bff6a Binary files /dev/null and b/ground/gcs/src/plugins/config/images/tinyfish_top.png differ diff --git a/ground/gcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/gcs/src/plugins/setupwizard/pages/controllerpage.cpp index abd5c6000..3d4368d79 100644 --- a/ground/gcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/gcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -128,6 +128,9 @@ SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() case 0x1005: return SetupWizard::CONTROLLER_PIKOBLX; + + case 0x1006: + return SetupWizard::CONTROLLER_TINYFISH; default: return SetupWizard::CONTROLLER_UNKNOWN; @@ -258,6 +261,10 @@ void ControllerPage::connectionStatusChanged() ui->boardImg->setPixmap(boardPic.scaled(picSize, Qt::KeepAspectRatio)); break; + case SetupWizard::CONTROLLER_TINYFISH: + boardPic.load(":/configgadget/images/tinyfish_top.png"); + ui->boardImg->setPixmap(boardPic.scaled(picSize, Qt::KeepAspectRatio)); + break; default: ui->boardImg->setPixmap(QPixmap()); break; diff --git a/ground/gcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/gcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 6fe2eac87..bb5885364 100644 --- a/ground/gcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/gcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -58,7 +58,7 @@ class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_SPARKY2, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4, CONTROLLER_SPRACINGF3, CONTROLLER_SPRACINGF3EVO, CONTROLLER_PIKOBLX }; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_SPARKY2, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4, CONTROLLER_SPRACINGF3, CONTROLLER_SPRACINGF3EVO, CONTROLLER_PIKOBLX, CONTROLLER_TINYFISH }; enum VEHICLE_TYPE { VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE }; enum VEHICLE_SUB_TYPE { MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, MULTI_ROTOR_QUAD_H, MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 5ca4ce966..cdf94d760 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -100,6 +100,7 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/hwspracingf3settings.xml \ $${UAVOBJ_XML_DIR}/hwspracingf3evosettings.xml \ $${UAVOBJ_XML_DIR}/hwpikoblxsettings.xml \ + $${UAVOBJ_XML_DIR}/hwtinyfishsettings.xml \ $${UAVOBJ_XML_DIR}/i2cstats.xml \ $${UAVOBJ_XML_DIR}/magsensor.xml \ $${UAVOBJ_XML_DIR}/magstate.xml \ diff --git a/ground/gcs/src/plugins/uavobjectutil/devicedescriptorstruct.h b/ground/gcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index 169bc8a46..b8d656b49 100644 --- a/ground/gcs/src/plugins/uavobjectutil/devicedescriptorstruct.h +++ b/ground/gcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -74,6 +74,9 @@ public: case 0x1005: return QString("PikoBLX"); + + case 0x1006: + return QString("tinyFISH"); default: return QString(""); diff --git a/ground/gcs/src/plugins/uploader/devicewidget.cpp b/ground/gcs/src/plugins/uploader/devicewidget.cpp index 807a08655..557cf1bbc 100644 --- a/ground/gcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/gcs/src/plugins/uploader/devicewidget.cpp @@ -121,6 +121,10 @@ void DeviceWidget::populate() // Sparky2 devicePic.load(":/uploader/images/gcs-board-sparky2.png"); break; + case 0x1006: + // tinyFISH + devicePic.load(":/uploader/images/gcs-board-tinyfish.png"); + break; default: // Clear devicePic.load(""); diff --git a/ground/gcs/src/plugins/uploader/images/gcs-board-tinyfish.png b/ground/gcs/src/plugins/uploader/images/gcs-board-tinyfish.png new file mode 100644 index 000000000..1f30bff6a Binary files /dev/null and b/ground/gcs/src/plugins/uploader/images/gcs-board-tinyfish.png differ diff --git a/ground/gcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/gcs/src/plugins/uploader/runningdevicewidget.cpp index 50d5490e7..0800b9be9 100644 --- a/ground/gcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/gcs/src/plugins/uploader/runningdevicewidget.cpp @@ -101,6 +101,10 @@ void RunningDeviceWidget::populate() // Sparky2 devicePic.load(":/uploader/images/gcs-board-sparky2.png"); break; + case 0x1006: + // tinyFISH + devicePic.load(":/uploader/images/gcs-board-tinyfish.png"); + break; default: // Clear devicePic.load(""); diff --git a/ground/gcs/src/plugins/uploader/uploader.qrc b/ground/gcs/src/plugins/uploader/uploader.qrc index 5c9e8eb26..cd5155561 100644 --- a/ground/gcs/src/plugins/uploader/uploader.qrc +++ b/ground/gcs/src/plugins/uploader/uploader.qrc @@ -20,5 +20,6 @@ images/gcs-board-revo.png images/gcs-board-nano.png images/gcs-board-sparky2.png + images/gcs-board-tinyfish.png diff --git a/ground/gcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/gcs/src/plugins/uploader/uploadergadgetwidget.cpp index 441edf1f0..bfecfc154 100644 --- a/ground/gcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/gcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -797,6 +797,9 @@ bool UploaderGadgetWidget::autoUpdate(bool erase) case 0x1005: filename = "fw_pikoblx"; break; + case 0x1006: + filename = "fw_tinyfish"; + break; default: emit progressUpdate(FAILURE, QVariant(tr("Unknown board id '0x%1'").arg(QString::number(m_dfu->devices[0].ID, 16)))); emit autoUpdateFailed(); diff --git a/shared/uavobjectdefinition/hwtinyfishsettings.xml b/shared/uavobjectdefinition/hwtinyfishsettings.xml new file mode 100644 index 000000000..6a268ddde --- /dev/null +++ b/shared/uavobjectdefinition/hwtinyfishsettings.xml @@ -0,0 +1,13 @@ + + + fishpepper.de tinyFISH Flight Controller hardware configuration + + + + + + + + + +