diff --git a/Makefile b/Makefile index c3667097f..d604f5541 100644 --- a/Makefile +++ b/Makefile @@ -214,9 +214,10 @@ export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics DIRS += $(OPGCSSYNTHDIR) # Define supported board lists -ALL_BOARDS := oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum revonano +ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum revonano # Short names of each board (used to display board name in parallel builds) +coptercontrol_short := 'cc ' oplinkmini_short := 'oplm' revolution_short := 'revo' osd_short := 'osd ' diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 0517892ff..b2265abf6 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -105,8 +105,9 @@ static void stabilizationOuterloopTask() int t; float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); StabilizationStatusOuterLoopOptions newThrustMode = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]; - bool reinit = (newThrustMode != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]); + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES // Trigger a disable message to the alt hold on reinit to prevent that loop from running when not in use. if (reinit) { if (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE || @@ -117,17 +118,20 @@ static void stabilizationOuterloopTask() } } } +#endif // update previous mode previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = newThrustMode; // calculate the thrust desired switch (newThrustMode) { +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit); break; case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit); break; +#endif case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: default: diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 39d681a68..d386d21b3 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -95,6 +95,9 @@ #define STATS_UPDATE_PERIOD_MS 4000 #define CONNECTION_TIMEOUT_MS 8000 +#ifdef PIOS_INCLUDE_RFM22B +#define HAS_RADIO +#endif // Private types typedef struct { // Determine port on which to communicate telemetry information @@ -111,12 +114,14 @@ typedef struct { // Telemetry stream UAVTalkConnection uavTalkCon; } channelContext; - +#ifdef HAS_RADIO // Main telemetry channel static channelContext localChannel; static int32_t transmitLocalData(uint8_t *data, int32_t length); static void registerLocalObject(UAVObjHandle obj); static uint32_t localPort(); +static void updateSettings(channelContext *channel); +#endif // OPLink telemetry channel static channelContext radioChannel; @@ -150,7 +155,6 @@ static int32_t setLoggingPeriod( int32_t updatePeriodMs); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); -static void updateSettings(channelContext *channel); /** * Initialise the telemetry module @@ -159,6 +163,7 @@ static void updateSettings(channelContext *channel); */ int32_t TelemetryStart(void) { +#ifdef HAS_RADIO // Only start the local telemetry tasks if needed if (localPort()) { UAVObjIterate(®isterLocalObject); @@ -187,8 +192,8 @@ int32_t TelemetryStart(void) PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, localChannel.rxTaskHandle); } - - // Start the telemetry tasks associated with Radio/USB +#endif /* ifdef HAS_RADIO */ + // Start the telemetry tasks associated with Radio/USB UAVObjIterate(®isterRadioObject); // Listen to objects of interest @@ -229,9 +234,6 @@ void TelemetryInitializeChannel(channelContext *channel) sizeof(UAVObjEvent)); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - // Initialise UAVTalk - channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData); - // Create periodic event that will be used to update the telemetry stats UAVObjEvent ev; memset(&ev, 0, sizeof(UAVObjEvent)); @@ -267,7 +269,7 @@ int32_t TelemetryInitialize(void) radio_port = PIOS_COM_RF; } #else /* PIOS_INCLUDE_RFM22B */ - radio_port = 0; + radio_port = PIOS_COM_TELEM_RF; #endif /* PIOS_INCLUDE_RFM22B */ FlightTelemetryStatsInitialize(); @@ -279,10 +281,9 @@ int32_t TelemetryInitialize(void) // Reset link stats txErrors = 0; txRetries = 0; - +#ifdef HAS_RADIO // Set channel port handlers localChannel.getPort = localPort; - radioChannel.getPort = radioPort; // Set the local telemetry baud rate updateSettings(&localChannel); @@ -295,6 +296,9 @@ int32_t TelemetryInitialize(void) localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); } +#endif + // Set channel port handlers + radioChannel.getPort = radioPort; // Initialise channel TelemetryInitializeChannel(&radioChannel); // Initialise UAVTalk @@ -304,7 +308,7 @@ int32_t TelemetryInitialize(void) } MODULE_INITCALL(TelemetryInitialize, TelemetryStart); - +#ifdef HAS_RADIO /** * Register a new object, adds object to local list and connects the queue depending on the object's * telemetry settings. @@ -327,7 +331,7 @@ static void registerLocalObject(UAVObjHandle obj) EV_NONE); } } - +#endif static void registerRadioObject(UAVObjHandle obj) { if (UAVObjIsMetaobject(obj)) { @@ -635,7 +639,7 @@ static void telemetryRxTask(void *parameters) } } - +#ifdef HAS_RADIO /** * Determine the port to be used for communication on the telemetry channel * \return com port number @@ -644,7 +648,7 @@ static uint32_t localPort() { return PIOS_COM_TELEM_RF; } - +#endif /** * Determine the port to be used for communication on the radio channel @@ -664,7 +668,7 @@ static uint32_t radioPort() return port; } - +#ifdef HAS_RADIO /** * Transmit data buffer to the modem or USB port. * \param[in] data Data buffer to send @@ -682,7 +686,7 @@ static int32_t transmitLocalData(uint8_t *data, int32_t length) return -1; } - +#endif /** * Transmit data buffer to the radioport. @@ -804,9 +808,10 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true); - UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true); - + UAVTalkGetStats(radioChannel.uavTalkCon, &utalkStats, true); +#ifdef HAS_RADIO + UAVTalkAddStats(localChannel.uavTalkCon, &utalkStats, true); +#endif // Get object data FlightTelemetryStatsGet(&flightStats); GCSTelemetryStatsGet(&gcsStats); @@ -888,6 +893,7 @@ static void updateTelemetryStats() } } +#ifdef HAS_RADIO /** * Update the telemetry settings, called on startup. * FIXME: This should be in the TelemetrySettings object. But objects @@ -931,7 +937,7 @@ static void updateSettings(channelContext *channel) } } - +#endif /* ifdef HAS_RADIO */ /** * @} * @} diff --git a/flight/pios/inc/pios_usb_defs.h b/flight/pios/inc/pios_usb_defs.h index 9078e1abb..164d31fb3 100644 --- a/flight/pios/inc/pios_usb_defs.h +++ b/flight/pios/inc/pios_usb_defs.h @@ -353,6 +353,7 @@ enum usb_cdc_notification { enum usb_product_ids { USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A, + USB_PRODUCT_ID_COPTERCONTROL = 0x415B, USB_PRODUCT_ID_OPLINK = 0x415C, USB_PRODUCT_ID_CC3D = 0x415D, USB_PRODUCT_ID_REVOLUTION = 0x415E, @@ -363,9 +364,9 @@ enum usb_product_ids { enum usb_op_board_ids { USB_OP_BOARD_ID_OPENPILOT_MAIN = 1, /* Board ID 2 may be unused or AHRS */ - USB_OP_BOARD_ID_OPLINK = 3, - /* USB_OP_BOARD_ID_COPTERCONTROL = 4, */ - USB_OP_BOARD_ID_REVOLUTION = 5, + USB_OP_BOARD_ID_OPLINK = 3, + USB_OP_BOARD_ID_COPTERCONTROL = 4, + USB_OP_BOARD_ID_REVOLUTION = 5, USB_OP_BOARD_ID_OSD = 6, } __attribute__((packed)); diff --git a/flight/targets/boards/coptercontrol/board-info.mk b/flight/targets/boards/coptercontrol/board-info.mk new file mode 100644 index 000000000..d58f3d71d --- /dev/null +++ b/flight/targets/boards/coptercontrol/board-info.mk @@ -0,0 +1,23 @@ +BOARD_TYPE := 0x04 +BOARD_REVISION := 0x02 +BOOTLOADER_VERSION := 0x04 +HW_TYPE := 0x01 + +MCU := cortex-m3 +CHIP := STM32F103CBT +BOARD := STM32103CB_CC_Rev1 +MODEL := MD +MODEL_SUFFIX := _CC + +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 := 0x00003000 # Should include BD_INFO region +FW_BANK_BASE := 0x08003000 # Start of firmware flash +FW_BANK_SIZE := 0x0001D000 # Should include FW_DESC_SIZE + +FW_DESC_SIZE := 0x00000064 + +OSCILLATOR_FREQ := 8000000 diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c new file mode 100644 index 000000000..17981095f --- /dev/null +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -0,0 +1,1525 @@ +/** + ****************************************************************************** + * @file board_hw_defs.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the CopterControl 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 + */ + +#define BOARD_REVISION_CC 1 +#define BOARD_REVISION_CC3D 2 + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_gpio pios_leds_cc[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + .active_low = true + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg_cc = { + .gpios = pios_leds_cc, + .num_gpios = NELEMENTS(pios_leds_cc), +}; + +static const struct pios_gpio pios_leds_cc3d[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + .remap = GPIO_Remap_SWJ_JTAGDisable, + .active_low = true + }, +}; + +static const struct pios_gpio_cfg pios_led_cfg_cc3d = { + .gpios = pios_leds_cc3d, + .num_gpios = NELEMENTS(pios_leds_cc3d), +}; + +const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision) +{ + switch (board_revision) { + case BOARD_REVISION_CC: return &pios_led_cfg_cc; + + case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d; + + default: return NULL; + } +} + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Gyro interface */ +void PIOS_SPI_gyro_irq_handler(void); +void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler"))); +static const struct pios_spi_cfg pios_spi_gyro_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, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 1, + .ssel = { + { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, +}; + +static uint32_t pios_spi_gyro_id; +void PIOS_SPI_gyro_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_gyro_id); +} + + +/* Flash/Accel Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_flash_accel_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler"))); +static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = { + .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_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, +}; + +static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = { + .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_8, + }, + .use_crc = false, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .slave_count = 2, + .ssel = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + } + },{ + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + } + }, +}; + +static uint32_t pios_spi_flash_accel_id; +void PIOS_SPI_flash_accel_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_FLASH) +#include "pios_flashfs_logfs_priv.h" +#include "pios_flash_jedec_priv.h" + +static const struct flashfs_logfs_cfg flashfs_w25x_cfg = { + .fs_magic = 0x99abcdef, + .total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00001000, /* 4K bytes */ + .page_size = 0x00000100, /* 256 bytes */ +}; + + +static const struct flashfs_logfs_cfg flashfs_m25p_cfg = { + .fs_magic = 0x99abceef, + .total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */ + .arena_size = 0x00010000, /* 256 * slot size */ + .slot_size = 0x00000100, /* 256 bytes */ + + .start_offset = 0, /* start at the beginning of the chip */ + .sector_size = 0x00010000, /* 64K bytes */ + .page_size = 0x00000100, /* 256 bytes */ +}; + +#include "pios_flash.h" + +#endif /* PIOS_INCLUDE_FLASH */ + +/* + * ADC system + */ +#if defined(PIOS_INCLUDE_ADC) +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +void PIOS_ADC_handler() +{ + PIOS_ADC_DMA_Handler(); +} +#endif /* if defined(PIOS_INCLUDE_ADC) */ + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_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_1_2_3_4_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_1_2_3_4_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_1_2_3_4_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_1_2_3_4_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_channel pios_tim_rcvrport_all_channels[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + +static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + + +static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + + // Receiver port pins + // S3-S6 inputs are used as outputs in this case + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + +static const struct pios_tim_channel pios_tim_ppm_flexi_port = { + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, +}; + +#if defined(PIOS_INCLUDE_USART) + +#include "pios_usart_priv.h" + +static const struct pios_usart_cfg pios_usart_generic_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +#if defined(PIOS_INCLUDE_DSM) +/* + * Spektrum/JR DSM USART + */ +#include + +static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static const struct pios_dsm_cfg pios_dsm_main_cfg = { + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { + .bind = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, +}; + +#endif /* PIOS_INCLUDE_DSM */ + + +#if defined(PIOS_INCLUDE_SBUS) +/* + * S.Bus USART + */ +#include + +static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 100000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_Even, + .USART_StopBits = USART_StopBits_2, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static const struct pios_sbus_cfg pios_sbus_cfg = { + /* Inverter configuration */ + .inv = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .gpio_clk_func = RCC_APB2PeriphClockCmd, + .gpio_clk_periph = RCC_APB2Periph_GPIOB, + .gpio_inv_enable = Bit_SET, +}; + +#endif /* PIOS_INCLUDE_SBUS */ + +/* + * HK OSD + */ +static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 57600, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + + +#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_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_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_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), +}; + +const struct pios_servo_cfg pios_servo_rcvr_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_rcvrport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +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, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, +}; + +const struct pios_ppm_cfg pios_ppm_pin8_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[5], + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM */ + +#if defined(PIOS_INCLUDE_PPM_FLEXI) +#include + +const struct pios_ppm_cfg pios_ppm_flexi_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_flexi_port, + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + +/* + * PWM Inputs + */ +#if defined(PIOS_INCLUDE_PWM) +#include + +const struct pios_pwm_cfg pios_pwm_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_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), +}; + +const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Leave the first channel for PPM use and use the rest for PWM */ + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, +}; + +#endif /* if defined(PIOS_INCLUDE_PWM) */ + + +/* + * SONAR Inputs + */ +#if defined(PIOS_INCLUDE_HCSR04) +#include + +static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = { + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, +}; + +const struct pios_hcsr04_cfg pios_hcsr04_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_hcsr04_port_all_channels, + .num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels), + .trigger = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; +#endif /* if defined(PIOS_INCLUDE_HCSR04) */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_flexi_adapter_ev_irq_handler(void); +void PIOS_I2C_flexi_adapter_er_irq_handler(void); +void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { + .regs = I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_flexi_adapter_id; +void PIOS_I2C_flexi_adapter_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); +} + +void PIOS_I2C_flexi_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_INCLUDE_GCSRCVR) +#include "pios_gcsrcvr_priv.h" +#endif /* PIOS_INCLUDE_GCSRCVR */ + +#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_cc = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false +}; + +static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .vsense = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .vsense_active_low = false +}; + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" + +#endif /* PIOS_INCLUDE_USB */ + +#if defined(PIOS_INCLUDE_COM_MSG) + +#include + +#endif /* PIOS_INCLUDE_COM_MSG */ + +#if defined(PIOS_INCLUDE_USB_HID) +#include + +const struct pios_usb_hid_cfg pios_usb_hid_cfg = { + .data_if = 2, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; + +#endif /* PIOS_INCLUDE_USB_HID */ + +#if defined(PIOS_INCLUDE_USB_RCTX) +#include + +const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { + .data_if = 2, + .data_tx_ep = 1, +}; + +#endif /* PIOS_INCLUDE_USB_RCTX */ + +#if defined(PIOS_INCLUDE_USB_CDC) +#include + +const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { + .ctrl_if = 0, + .ctrl_tx_ep = 2, + + .data_if = 1, + .data_rx_ep = 3, + .data_tx_ep = 3, +}; +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/Makefile b/flight/targets/boards/coptercontrol/bootloader/Makefile new file mode 100644 index 000000000..7c575411b --- /dev/null +++ b/flight/targets/boards/coptercontrol/bootloader/Makefile @@ -0,0 +1,32 @@ +# +# 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 OPENPILOT_IS_COOL + $(error Top level Makefile must be used to build this target) +endif + +## The standard CMSIS startup +SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c + +include ../board-info.mk +include $(ROOT_DIR)/make/firmware-defs.mk +include $(ROOT_DIR)/make/boot-defs.mk +include $(ROOT_DIR)/make/common-defs.mk + +$(info Making bootloader for CopterControl, board revision $(BOARD_REVISION)) +$(info Use BOARD_REVISION=1 for CC, BOARD_REVISION=2 for CC3D (default)) diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/common.h b/flight/targets/boards/coptercontrol/bootloader/inc/common.h new file mode 100644 index 000000000..e64b89014 --- /dev/null +++ b/flight/targets/boards/coptercontrol/bootloader/inc/common.h @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * @addtogroup CopterControlBL CopterControl BootLoader + * @brief These files contain the code to the CopterControl Bootloader. + * + * @{ + * @file common.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This file contains various common defines for the 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 + */ +#ifndef COMMON_H_ +#define COMMON_H_ + +// #include "board.h" + +typedef enum { + start, keepgoing, +} DownloadAction; + +/**************************************************/ +/* OP_DFU states */ +/**************************************************/ + +typedef enum { + DFUidle, // 0 + uploading, // 1 + wrong_packet_received, // 2 + too_many_packets, // 3 + too_few_packets, // 4 + Last_operation_Success, // 5 + downloading, // 6 + BLidle, // 7 + Last_operation_failed, // 8 + uploadingStarting, // 9 + outsideDevCapabilities, // 10 + CRC_Fail, // 11 + failed_jump, +// 12 +} DFUStates; +/**************************************************/ +/* OP_DFU commands */ +/**************************************************/ +typedef enum { + Reserved, // 0 + Req_Capabilities, // 1 + Rep_Capabilities, // 2 + EnterDFU, // 3 + JumpFW, // 4 + Reset, // 5 + Abort_Operation, // 6 + Upload, // 7 + Op_END, // 8 + Download_Req, // 9 + Download, // 10 + Status_Request, // 11 + Status_Rep +// 12 +} DFUCommands; + +typedef enum { + High_Density, Medium_Density +} DeviceType; +/**************************************************/ +/* OP_DFU transfer types */ +/**************************************************/ +typedef enum { + FW, // 0 + Descript +// 2 +} DFUTransfer; +/**************************************************/ +/* OP_DFU transfer port */ +/**************************************************/ +typedef enum { + Usb, // 0 + Serial +// 2 +} DFUPort; +/**************************************************/ +/* OP_DFU programable programable HW types */ +/**************************************************/ +typedef enum { + Self_flash, // 0 + Remote_flash_via_spi +// 1 +} DFUProgType; +/**************************************************/ +/* OP_DFU programable sources */ +/**************************************************/ +#define USB 0 +#define SPI 1 + +#define DownloadDelay 100000 + +#define MAX_DEL_RETRYS 3 +#define MAX_WRI_RETRYS 3 + +#endif /* COMMON_H_ */ diff --git a/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h new file mode 100644 index 000000000..43182e435 --- /dev/null +++ b/flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup CopterControlBL CopterControl BootLoader + * @brief These files contain the code to the CopterControl 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/coptercontrol/bootloader/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h new file mode 100644 index 000000000..5a40bf4cf --- /dev/null +++ b/flight/targets/boards/coptercontrol/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_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, 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/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c new file mode 100644 index 000000000..15068627c --- /dev/null +++ b/flight/targets/boards/coptercontrol/bootloader/main.c @@ -0,0 +1,223 @@ +/** + ****************************************************************************** + * @addtogroup CopterControlBL CopterControl BootLoader + * @brief These files contain the code to the CopterControl Bootloader. + * + * @{ + * @file main.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This is the file with the main function of the OpenPilot 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 + +/* Prototype of PIOS_Board_Init() function */ +extern void PIOS_Board_Init(void); +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; + + if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* 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; + 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/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c new file mode 100644 index 000000000..e0a63193a --- /dev/null +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -0,0 +1,106 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the CopterControl 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 + +/* + * 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(FLASH_PrefetchBuffer_Enable); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + + /* Delay system */ + PIOS_DELAY_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_Assert(led_cfg); + PIOS_LED_Init(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; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } +#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_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/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile new file mode 100644 index 000000000..221820cf8 --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -0,0 +1,152 @@ +# +# 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 OPENPILOT_IS_COOL + $(error Top level Makefile must be used to build this target) +endif + +include ../board-info.mk +include $(ROOT_DIR)/make/firmware-defs.mk + +# ARM DSP library +USE_DSP_LIB ?= NO + +# Set to YES to build a FW version that will erase data flash memory +ERASE_FLASH ?= NO + +# Set to yes to include Gcsreceiver module +GCSRECEIVER ?= NO + +# Enable Diag tasks ? +DIAG_TASKS ?= NO + +# List of mandatory modules to include +MODULES += Attitude +MODULES += Stabilization +MODULES += Actuator +MODULES += Receiver +MODULES += ManualControl +MODULES += FirmwareIAP +MODULES += Telemetry + +# List of optional modules to include +OPTMODULES += CameraStab +OPTMODULES += ComUsbBridge +OPTMODULES += GPS +OPTMODULES += TxPID +OPTMODULES += Osd/osdoutput +#OPTMODULES += Altitude +#OPTMODULES += Fault + +SRC += $(FLIGHTLIB)/notification.c + +# Include all camera options +CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + + + +# Erase flash firmware should be buildable from command line +ifeq ($(ERASE_FLASH), YES) + CDEFS += -DERASE_FLASH +endif + +# List C source files here (C dependencies are automatically generated). +# Use file-extension c for "c-only"-files +ifndef TESTAPP + ## The standard CMSIS startup + SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c + + ## Application Core + SRC += ../pios_usb_board_data.c + SRC += $(OPMODULEDIR)/System/systemmod.c + SRC += $(OPSYSTEM)/coptercontrol.c + SRC += $(OPSYSTEM)/pios_board.c + SRC += $(FLIGHTLIB)/alarms.c + SRC += $(FLIGHTLIB)/instrumentation.c + SRC += $(OPUAVTALK)/uavtalk.c + SRC += $(OPUAVOBJ)/uavobjectmanager.c + SRC += $(OPUAVOBJ)/uavobjectpersistence.c + SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c + SRC += $(PIOSCOMMON)/pios_flash_jedec.c + SRC += $(OPUAVOBJ)/eventdispatcher.c + + ## UAVObjects + SRC += $(OPUAVSYNTHDIR)/accessorydesired.c + SRC += $(OPUAVSYNTHDIR)/objectpersistence.c + SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c + SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c + SRC += $(OPUAVSYNTHDIR)/faultsettings.c + SRC += $(OPUAVSYNTHDIR)/flightstatus.c + SRC += $(OPUAVSYNTHDIR)/systemstats.c + SRC += $(OPUAVSYNTHDIR)/systemalarms.c + SRC += $(OPUAVSYNTHDIR)/systemsettings.c + SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c + SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c + SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c + SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c + SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c + SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c + SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c + SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c + SRC += $(OPUAVSYNTHDIR)/actuatordesired.c + SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c + SRC += $(OPUAVSYNTHDIR)/accelstate.c + SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c + SRC += $(OPUAVSYNTHDIR)/gyrostate.c + SRC += $(OPUAVSYNTHDIR)/attitudestate.c + SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c + SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c + SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c + SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c + SRC += $(OPUAVSYNTHDIR)/mixersettings.c + SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c + SRC += $(OPUAVSYNTHDIR)/attitudesettings.c + SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c + SRC += $(OPUAVSYNTHDIR)/cameradesired.c + SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c + SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c + SRC += $(OPUAVSYNTHDIR)/gpssettings.c + SRC += $(OPUAVSYNTHDIR)/hwsettings.c + SRC += $(OPUAVSYNTHDIR)/receiveractivity.c + SRC += $(OPUAVSYNTHDIR)/receiverstatus.c + SRC += $(OPUAVSYNTHDIR)/mixerstatus.c + SRC += $(OPUAVSYNTHDIR)/ratedesired.c + SRC += $(OPUAVSYNTHDIR)/txpidsettings.c + SRC += $(OPUAVSYNTHDIR)/txpidstatus.c + SRC += $(OPUAVSYNTHDIR)/mpugyroaccelsettings.c + # Command line option for Gcsreceiver module + ifeq ($(GCSRECEIVER), YES) + SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c + endif + # Enable Diag tasks and UAVOs needed + ifeq ($(DIAG_TASKS), YES) + CDEFS += -DDIAG_TASKS + SRC += $(OPUAVSYNTHDIR)/taskinfo.c + SRC += $(OPUAVSYNTHDIR)/callbackinfo.c + SRC += $(OPUAVSYNTHDIR)/perfcounter.c + SRC += $(OPUAVSYNTHDIR)/i2cstats.c + endif +else + ## Test Code + SRC += $(OPTESTS)/test_common.c + SRC += $(OPTESTS)/$(TESTAPP).c +endif + +include $(ROOT_DIR)/make/apps-defs.mk +include $(ROOT_DIR)/make/common-defs.mk diff --git a/flight/targets/boards/coptercontrol/firmware/coptercontrol.c b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c new file mode 100644 index 000000000..e748f6317 --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/coptercontrol.c @@ -0,0 +1,117 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @brief These files are the core system files of OpenPilot. + * They are the ground layer just above PiOS. In practice, OpenPilot actually starts + * in the main() function of openpilot.c + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @brief This is where the OP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file openpilot.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Sets up and runs main OpenPilot 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 + */ + +#include "inc/openpilot.h" +#include +#include + +/* Task Priorities */ +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) + +/* Global Variables */ + +/* Prototype of PIOS_Board_Init() function */ +extern void PIOS_Board_Init(void); +extern void Stack_Change(void); + +/** + * 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() */ + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); + +#ifdef ERASE_FLASH + PIOS_Flash_Jedec_EraseChip(); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + while (1) { + ; + } +#endif + + /* Initialize modules */ + MODULE_INITIALISE_ALL + /* swap the stack to use the IRQ stack */ + Stack_Change(); + + /* 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/coptercontrol/firmware/inc/FreeRTOSConfig.h b/flight/targets/boards/coptercontrol/firmware/inc/FreeRTOSConfig.h new file mode 100644 index 000000000..429d86ade --- /dev/null +++ b/flight/targets/boards/coptercontrol/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)48) +#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/coptercontrol/firmware/inc/openpilot.h b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h new file mode 100644 index 000000000..5576baca6 --- /dev/null +++ b/flight/targets/boards/coptercontrol/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/coptercontrol/firmware/inc/pios_board_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h new file mode 100644 index 000000000..16364d7fa --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_board_posix.h @@ -0,0 +1,81 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. + * @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 + + +// ------------------------ +// PIOS_LED +// ------------------------ +// #define PIOS_LED_LED1_GPIO_PORT GPIOC +// #define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 +// #define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC +// #define PIOS_LED_LED2_GPIO_PORT GPIOC +// #define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 +// #define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC +#define PIOS_LED_NUM 2 +// #define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } +// #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } +// #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } + + +// ------------------------- +// COM +// +// See also pios_board_posix.c +// ------------------------- +// #define PIOS_USART_TX_BUFFER_SIZE 256 +#define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE + +#define PIOS_COM_TELEM_RF 0 +#define PIOS_COM_GPS 1 +#define PIOS_COM_TELEM_USB 2 + +#ifdef PIOS_ENABLE_AUX_UART +#define PIOS_COM_AUX 3 +#define PIOS_COM_DEBUG PIOS_COM_AUX +#endif + +/** + * glue macros for file IO + * STM32 uses DOSFS for file IO + */ +#define PIOS_FOPEN_READ(filename, file) (file = fopen((char *)filename, "r")) == NULL + +#define PIOS_FOPEN_WRITE(filename, file) (file = fopen((char *)filename, "w")) == NULL + +#define PIOS_FREAD(file, bufferadr, length, resultadr) (*resultadr = fread((uint8_t *)bufferadr, 1, length, *file)) != length + +#define PIOS_FWRITE(file, bufferadr, length, resultadr) *resultadr = fwrite((uint8_t *)bufferadr, 1, length, *file) + + +#define PIOS_FCLOSE(file) fclose(file) + +#define PIOS_FUNLINK(file) unlink((char *)filename) + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h new file mode 100644 index 000000000..a371b908f --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -0,0 +1,188 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @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_BMP085 */ +/* #define PIOS_INCLUDE_MS5611 */ +/* #define PIOS_INCLUDE_MPXV */ +/* #define PIOS_INCLUDE_ETASV3 */ +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + +/* 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_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_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 820 +#define PIOS_MANUAL_STACK_SIZE 735 +#define PIOS_RECEIVER_STACK_SIZE 620 +#define PIOS_STABILIZATION_STACK_SIZE 400 + +#ifdef DIAG_TASKS +#define PIOS_SYSTEM_STACK_SIZE 740 +#else +#define PIOS_SYSTEM_STACK_SIZE 660 +#endif +#define PIOS_TELEM_RX_STACK_SIZE 410 +#define PIOS_TELEM_TX_STACK_SIZE 560 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 95 + +/* 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/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h new file mode 100644 index 000000000..4e8aa47c6 --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * + * @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. + * @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_POSIX_H +#define PIOS_CONFIG_POSIX_H + + +/* Enable/Disable PiOS Modules */ +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_FREERTOS +#define PIOS_INCLUDE_CALLBACKSCHEDULER +#define PIOS_INCLUDE_TASK_MONITOR +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_UDP +#define PIOS_INCLUDE_SERVO + + +/* Defaults for Logging */ +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 + +#endif /* PIOS_CONFIG_POSIX_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h new file mode 100644 index 000000000..1fac7c659 --- /dev/null +++ b/flight/targets/boards/coptercontrol/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_COPTERCONTROL +#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) +#define PIOS_USB_BOARD_SN_SUFFIX "+FW" + +#endif /* PIOS_USB_BOARD_DATA_H */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c new file mode 100644 index 000000000..474a21330 --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -0,0 +1,920 @@ +/** + ***************************************************************************** + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * @brief Defines board specific static initializers for hardware for the CopterControl 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 + +#ifdef PIOS_INCLUDE_INSTRUMENTATION +#include +#endif +#if defined(PIOS_INCLUDE_ADXL345) +#include +#endif + +/* + * 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" + +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + +static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook(); +static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); + +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 + +#define PIOS_COM_GPS_RX_BUF_LEN 32 + +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 + +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 + +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 + +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) +#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 +uint32_t pios_com_debug_id; +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + +uint32_t pios_com_telem_rf_id; +uint32_t pios_com_telem_usb_id; +uint32_t pios_com_vcp_id; +uint32_t pios_com_gps_id; +uint32_t pios_com_bridge_id; +uint32_t pios_com_hkosd_id; + +uint32_t pios_usb_rctx_id; + +uintptr_t pios_uavo_settings_fs_id; +uintptr_t pios_user_fs_id = 0; +/** + * Configuration for MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +#include "pios_mpu6000_config.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line3, // 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_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 2 +}; +#endif /* PIOS_INCLUDE_MPU6000 */ + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +int32_t init_test; +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); + + const struct pios_board_info *bdinfo = &pios_board_info_blob; + +#if defined(PIOS_INCLUDE_LED) + const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); + PIOS_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 serial flash */ + + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { + PIOS_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { + PIOS_Assert(0); + } + break; + default: + PIOS_Assert(0); + } + +#endif + + uintptr_t flash_id; + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 1)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_w25x_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + case BOARD_REVISION_CC3D: + if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_flash_accel_id, 0)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id)) { + PIOS_DEBUG_Assert(0); + } + break; + default: + PIOS_DEBUG_Assert(0); + } + + /* 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(); + +#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); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + +#if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + + /* Flags to determine if various USB interfaces are advertised */ + bool usb_hid_present = false; + bool usb_cdc_present = false; + +#if defined(PIOS_INCLUDE_USB_CDC) + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; +#else + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; +#endif + + uint32_t pios_usb_id; + + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); + break; + case BOARD_REVISION_CC3D: + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); + break; + default: + PIOS_Assert(0); + } + +#if defined(PIOS_INCLUDE_USB_CDC) + + uint8_t hwsettings_usb_vcpport; + /* Configure the USB VCP port */ + HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); + + if (!usb_cdc_present) { + /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ + hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; + } + + switch (hwsettings_usb_vcpport) { + case HWSETTINGS_USB_VCPPORT_DISABLED: + break; + case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: +#if defined(PIOS_INCLUDE_COM) + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_COMBRIDGE: +#if defined(PIOS_INCLUDE_COM) + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + { + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + } +#endif /* PIOS_INCLUDE_USB_CDC */ + +#if defined(PIOS_INCLUDE_USB_HID) + /* Configure the usb HID port */ + uint8_t hwsettings_usb_hidport; + HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); + + if (!usb_hid_present) { + /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ + hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; + } + + switch (hwsettings_usb_hidport) { + case HWSETTINGS_USB_HIDPORT_DISABLED: + break; + case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: +#if defined(PIOS_INCLUDE_COM) + { + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: +#if defined(PIOS_INCLUDE_USB_RCTX) + { + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + break; + } + +#endif /* PIOS_INCLUDE_USB_HID */ + +#endif /* PIOS_INCLUDE_USB */ + + /* Configure the main IO port */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); + uint8_t hwsettings_cc_mainport; + HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); + + switch (hwsettings_cc_mainport) { + case HWSETTINGS_CC_MAINPORT_DISABLED: + break; + case HWSETTINGS_CC_MAINPORT_TELEMETRY: +#if defined(PIOS_INCLUDE_TELEMETRY_RF) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ + break; + case HWSETTINGS_CC_MAINPORT_SBUS: +#if defined(PIOS_INCLUDE_SBUS) + { + uint32_t pios_usart_sbus_id; + if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_id; + if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_SBUS */ + break; + case HWSETTINGS_CC_MAINPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_MAINPORT_DSM: +#if defined(PIOS_INCLUDE_DSM) + { + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_main_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + 0)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_MAINPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_MAINPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_main_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } + + /* Configure the flexi port */ + uint8_t hwsettings_cc_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); + + switch (hwsettings_cc_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: +#if defined(PIOS_INCLUDE_TELEMETRY_RF) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_TELEMETRY_RF */ + break; + case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, + tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + case HWSETTINGS_CC_FLEXIPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: +#if defined(PIOS_INCLUDE_PPM_FLEXI) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + break; + case HWSETTINGS_CC_FLEXIPORT_DSM: +#if defined(PIOS_INCLUDE_DSM) + { + uint32_t pios_usart_dsm_id; + if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_id; + if (PIOS_DSM_Init(&pios_dsm_id, + &pios_dsm_flexi_cfg, + &pios_usart_com_driver, + pios_usart_dsm_id, + hwsettings_DSMxBind)) { + PIOS_Assert(0); + } + + uint32_t pios_dsm_rcvr_id; + if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; + } +#endif /* PIOS_INCLUDE_DSM */ + break; + case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) + { + uint32_t pios_usart_generic_id; + if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_id, + NULL, 0, + tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#endif /* PIOS_INCLUDE_COM */ + break; + case HWSETTINGS_CC_FLEXIPORT_I2C: +#if defined(PIOS_INCLUDE_I2C) + { + if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_I2C */ + break; + case HWSETTINGS_CC_FLEXIPORT_OSDHK: + { + uint32_t pios_usart_hkosd_id; + if (PIOS_USART_Init(&pios_usart_hkosd_id, &pios_usart_hkosd_flexi_cfg)) { + PIOS_Assert(0); + } + + uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_HKOSD_TX_BUF_LEN); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_hkosd_id, &pios_usart_com_driver, pios_usart_hkosd_id, + NULL, 0, + tx_buffer, PIOS_COM_HKOSD_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + break; + } + + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); + + switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: +#if defined(PIOS_INCLUDE_HCSR04) + { + uint32_t pios_hcsr04_id; + PIOS_HCSR04_Init(&pios_hcsr04_id, &pios_hcsr04_cfg); + } +#endif + break; + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: +#if defined(PIOS_INCLUDE_PWM) + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: +#if defined(PIOS_INCLUDE_PPM) + { + uint32_t pios_ppm_id; + if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) { + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg); + } else { + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + } + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ + break; + case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: + /* This is a combination of PPM and PWM inputs */ +#if defined(PIOS_INCLUDE_PPM) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ +#if defined(PIOS_INCLUDE_PWM) + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + break; + } + +#if defined(PIOS_INCLUDE_GCSRCVR) + GCSReceiverInitialize(); + uint32_t pios_gcsrcvr_id; + PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ + + /* Remap AFIO pin for PB4 (Servo 5 Out)*/ + GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); + +#ifndef PIOS_ENABLE_DEBUG_PINS + switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) { + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: + PIOS_Servo_Init(&pios_servo_cfg); + break; + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + PIOS_Servo_Init(&pios_servo_rcvr_cfg); + break; + } +#else + PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); +#endif /* PIOS_ENABLE_DEBUG_PINS */ + + switch (bdinfo->board_rev) { + case BOARD_REVISION_CC: + // Revision 1 with invensense gyros, start the ADC +#if defined(PIOS_INCLUDE_ADC) + PIOS_ADC_Init(&pios_adc_cfg); +#endif +#if defined(PIOS_INCLUDE_ADXL345) + PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); +#endif + break; + case BOARD_REVISION_CC3D: + // Revision 2 with MPU6000 gyros, start a SPI interface and connect to it + GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); + +#if defined(PIOS_INCLUDE_MPU6000) + // Set up the SPI interface to the serial flash + if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { + PIOS_Assert(0); + } + PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); + PIOS_MPU6000_CONFIG_Configure(); + init_test = !PIOS_MPU6000_Driver.test(0); +#endif /* PIOS_INCLUDE_MPU6000 */ + + break; + default: + PIOS_Assert(0); + } + + /* 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); + + // Attach the board config check hook + SANITYCHECK_AttachHook(&CopterControlConfigHook); + // trigger a config check if actuatorsettings are updated + ActuatorSettingsInitialize(); + ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); +} + +SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook() +{ + // inhibit usage of oneshot for non supported RECEIVER port modes + uint8_t recmode; + + HwSettingsCC_RcvrPortGet(&recmode); + uint8_t flexiMode; + uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM]; + ActuatorSettingsBankModeGet(modes); + HwSettingsCC_FlexiPortGet(&flexiMode); + + switch ((HwSettingsCC_RcvrPortOptions)recmode) { + // Those modes allows oneshot usage + case HWSETTINGS_CC_RCVRPORT_DISABLEDONESHOT: + case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: + if ((recmode == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT || + flexiMode == HWSETTINGS_CC_FLEXIPORT_PPM) && + (modes[3] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || + modes[3] == ACTUATORSETTINGS_BANKMODE_ONESHOT125)) { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT; + } else { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + } + + // inhibit oneshot for the following modes + case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: + case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: + for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { + if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || + modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; + } + + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + } + } + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; +} +// trigger a configuration check if ActuatorSettings are changed. +void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + configuration_check(); +} + +/** + * @} + */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c new file mode 100644 index 000000000..7fa958a38 --- /dev/null +++ b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c @@ -0,0 +1,145 @@ +/** + ****************************************************************************** + * + * @file pios_board.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board specific static initializers for hardware for the OpenPilot 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 "inc/openpilot.h" +#include +#include +#include + +/** + * PIOS_Board_Init() + * initializes all the core systems on this specific hardware + * called from System/openpilot.c + */ +void PIOS_Board_Init(void) +{ + /* Delay system */ + PIOS_DELAY_Init(); + + /* Initialize the delayed callback library */ + PIOS_CALLBACKSCHEDULER_Initialize(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Initialize the task monitor */ + if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { + PIOS_Assert(0); + } + + /* Initialize the PiOS library */ + PIOS_COM_Init(); +} + + +const struct pios_udp_cfg pios_udp0_cfg = { + .ip = "0.0.0.0", + .port = 9000, +}; +const struct pios_udp_cfg pios_udp1_cfg = { + .ip = "0.0.0.0", + .port = 9001, +}; +const struct pios_udp_cfg pios_udp2_cfg = { + .ip = "0.0.0.0", + .port = 9002, +}; + +#ifdef PIOS_COM_AUX +/* + * AUX USART + */ +const struct pios_udp_cfg pios_udp3_cfg = { + .ip = "0.0.0.0", + .port = 9003, +}; +#endif + +/* + * Board specific number of devices. + */ +struct pios_udp_dev pios_udp_devs[] = { +#define PIOS_UDP_TELEM 0 + { + .cfg = &pios_udp0_cfg, + }, +#define PIOS_UDP_GPS 1 + { + .cfg = &pios_udp1_cfg, + }, +#define PIOS_UDP_LOCAL 2 + { + .cfg = &pios_udp2_cfg, + }, +#ifdef PIOS_COM_AUX +#define PIOS_UDP_AUX 3 + { + .cfg = &pios_udp3_cfg, + }, +#endif +}; + +uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); + +/* + * COM devices + */ + +/* + * Board specific number of devices. + */ +extern const struct pios_com_driver pios_serial_com_driver; +extern const struct pios_com_driver pios_udp_com_driver; + +struct pios_com_dev pios_com_devs[] = { + { + .id = PIOS_UDP_TELEM, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_GPS, + .driver = &pios_udp_com_driver, + }, + { + .id = PIOS_UDP_LOCAL, + .driver = &pios_udp_com_driver, + }, +#ifdef PIOS_COM_AUX + { + .id = PIOS_UDP_AUX, + .driver = &pios_udp_com_driver, + }, +#endif +}; + +const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); + +/** + * @} + */ diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h new file mode 100644 index 000000000..f3c3e34d3 --- /dev/null +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -0,0 +1,279 @@ +/** + ****************************************************************************** + * + * @file pios_board.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. + * @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 +// ------------------------ +// Timers and Channels Used +// ------------------------ +/* + Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4 + ------+-----------+-----------+-----------+---------- + TIM1 | Servo 4 | | | + TIM2 | RC In 5 | RC In 6 | Servo 6 | + TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4 + TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 + ------+-----------+-----------+-----------+---------- + */ + +// ------------------------ +// DMA Channels Used +// ------------------------ +/* Channel 1 - */ +/* Channel 2 - */ +/* Channel 3 - */ +/* Channel 4 - */ +/* Channel 5 - */ +/* Channel 6 - */ +/* Channel 7 - */ +/* Channel 8 - */ +/* Channel 9 - */ +/* Channel 10 - */ +/* Channel 11 - */ +/* Channel 12 - */ + +// ------------------------ +// 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 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 + +// ------------------------ +// TELEMETRY +// ------------------------ +#define TELEM_QUEUE_SIZE 10 + +// ------------------------ +// PIOS_LED +// ------------------------ +#define PIOS_LED_HEARTBEAT 0 + +// ------------------------- +// System Settings +// ------------------------- +#define PIOS_MASTER_CLOCK 72000000 +#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) + +// ------------------------- +// 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 1 +extern uint32_t pios_i2c_flexi_adapter_id; +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) + +// ------------------------ +// PIOS_BMP085 +// ------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 + +// ------------------------- +// SPI +// +// See also pios_board.c +// ------------------------- +#define PIOS_SPI_MAX_DEVS 2 + +// ------------------------- +// PIOS_USART +// ------------------------- +#define PIOS_USART_MAX_DEVS 2 + +// ------------------------- +// PIOS_COM +// +// See also pios_board.c +// ------------------------- +#define PIOS_COM_MAX_DEVS 3 + +extern uint32_t pios_com_telem_rf_id; +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) + +#if defined(PIOS_INCLUDE_GPS) +extern uint32_t pios_com_gps_id; +#define PIOS_COM_GPS (pios_com_gps_id) +#endif /* PIOS_INCLUDE_GPS */ + +extern uint32_t pios_com_bridge_id; +#define PIOS_COM_BRIDGE (pios_com_bridge_id) + +extern uint32_t pios_com_vcp_id; +#define PIOS_COM_VCP (pios_com_vcp_id) + +extern uint32_t pios_com_telem_usb_id; +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) + +#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) +extern uint32_t pios_com_debug_id; +#define PIOS_COM_DEBUG (pios_com_debug_id) +#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ + +extern uint32_t pios_com_hkosd_id; +#define PIOS_COM_OSDHK (pios_com_hkosd_id) + +// ------------------------- +// ADC +// PIOS_ADC_PinGet(0) = Gyro Z +// PIOS_ADC_PinGet(1) = Gyro Y +// PIOS_ADC_PinGet(2) = Gyro X +// ------------------------- +// #define PIOS_ADC_OVERSAMPLING_RATE 1 +#define PIOS_ADC_USE_TEMP_SENSOR 1 +#define PIOS_ADC_TEMP_SENSOR_ADC ADC1 +#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1 + +#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA4 (Gyro X) +#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_4 // ADC12_IN4 +#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_4 +#define PIOS_ADC_PIN1_ADC ADC2 +#define PIOS_ADC_PIN1_ADC_NUMBER 1 + +#define PIOS_ADC_PIN2_GPIO_PORT GPIOA // PA5 (Gyro Y) +#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_5 // ADC123_IN5 +#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_5 +#define PIOS_ADC_PIN2_ADC ADC1 +#define PIOS_ADC_PIN2_ADC_NUMBER 2 + +#define PIOS_ADC_PIN3_GPIO_PORT GPIOA // PA3 (Gyro Z) +#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_3 // ADC12_IN3 +#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_3 +#define PIOS_ADC_PIN3_ADC ADC2 +#define PIOS_ADC_PIN3_ADC_NUMBER 2 + +#define PIOS_ADC_NUM_PINS 3 + +#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT } +#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN } +#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL } +#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC } +#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER } +#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR) +#define PIOS_ADC_NUM_ADC_CHANNELS 2 +#define PIOS_ADC_USE_ADC2 1 +#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE) +#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8 +/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */ +/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */ +/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */ +/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */ +#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5 +/* Sample time: */ +/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */ +/* Tconv = 239.5 + 12.5 = 252 cycles = 18�s */ +/* (1 / (ADCCLK / CYCLES)) = Sample Time (�S) */ +#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_LOW + +// Currently analog acquistion hard coded at 480 Hz +// PCKL2 = HCLK / 16 +// ADCCLK = PCLK2 / 2 +#define PIOS_ADC_RATE (72.0e6f / 1.0f / 8.0f / 252.0f / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) +#define PIOS_ADC_MAX_OVERSAMPLING 48 + +#define PIOS_ADC_TEMPERATURE_PIN 0 + +// ------------------------ +// 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 12 + +// ------------------------- +// 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) + +// ------------------------- +// 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_DETECT_GPIO_PORT GPIOC +#define PIOS_USB_MAX_DEVS 1 +#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 + +#endif /* PIOS_BOARD_H */ diff --git a/flight/targets/boards/coptercontrol/pios_usb_board_data.c b/flight/targets/boards/coptercontrol/pios_usb_board_data.c new file mode 100644 index 000000000..20c49fdcf --- /dev/null +++ b/flight/targets/boards/coptercontrol/pios_usb_board_data.c @@ -0,0 +1,102 @@ +/** + ****************************************************************************** + * @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 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[28] = { + sizeof(usb_product_id), + USB_DESC_TYPE_STRING, + 'C', 0, + 'o', 0, + 'p', 0, + 't', 0, + 'e', 0, + 'r', 0, + 'C', 0, + 'o', 0, + 'n', 0, + 't', 0, + 'r', 0, + 'o', 0, + 'l', 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/openpilotgcs/src/plugins/config/cc_hw_settings.ui b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui new file mode 100644 index 000000000..e7c7ddf10 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui @@ -0,0 +1,658 @@ + + + CC_HW_Widget + + + + 0 + 0 + 646 + 596 + + + + Form + + + + + + 0 + + + + HW settings + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 232 + 232 + 232 + + + + + + + 232 + 232 + 232 + + + + + + + + QFrame::NoFrame + + + QFrame::Plain + + + true + + + + + 0 + 0 + 624 + 510 + + + + + 12 + + + 12 + + + 12 + + + 12 + + + + + + 0 + 0 + + + + + 0 + 50 + + + + Messages + + + + + + + 75 + true + + + + + + + Qt::AutoText + + + true + + + + + + + + + + + 0 + 0 + + + + + 500 + 0 + + + + + 500 + 16777215 + + + + + 75 + true + + + + Changes on this page only take effect after board reset or power cycle + + + Qt::AlignCenter + + + true + + + + + + + Qt::Vertical + + + + 20 + 25 + + + + + + + + + + + + Select the speed here. + + + + + + + + 55 + 0 + + + + GPS speed: + + + + + + + Telemetry speed: + + + + + + + Select the speed here. + + + + + + + Select the speed here. + + + + + + + + 55 + 0 + + + + ComUsbBridge speed: + + + + + + + GPS protocol : + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + + + + USB HID Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + + + + + USB VCP Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + false + + + + + + + Main Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + Flexi Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + Receiver Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 10 + + + + + + + + + + + + + + + + 4 + + + + + Qt::Horizontal + + + + 369 + 20 + + + + + + + + + 0 + 0 + + + + + 25 + 25 + + + + + 25 + 25 + + + + Takes you to the wiki page + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 25 + 25 + + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 232 + 232 + 232 + + + + + + + 232 + 232 + 232 + + + + + + + + Send to OpenPilot but don't write in SD. +Beware of not locking yourself out! + + + false + + + + + + Apply + + + + 16 + 16 + + + + false + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Applies and Saves all settings to SD. +Beware of not locking yourself out! + + + false + + + + + + Save + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui new file mode 100644 index 000000000..1a659982e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui @@ -0,0 +1,597 @@ + + + ccattitude + + + + 0 + 0 + 780 + 566 + + + + Form + + + + + + 0 + + + + Attitude + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 255 + 255 + 255 + + + + + + + 232 + 232 + 232 + + + + + + + + + 232 + 232 + 232 + + + + + + + 232 + 232 + 232 + + + + + + + + QFrame::NoFrame + + + QFrame::Plain + + + true + + + + + 0 + 0 + 758 + 486 + + + + + 12 + + + 12 + + + 12 + + + 12 + + + + + Rotate virtual attitude relative to board + + + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Roll + + + Qt::AlignCenter + + + + + + + -180 + + + 180 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + -180 + + + 180 + + + + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Yaw + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Pitch + + + Qt::AlignCenter + + + + + + + -90 + + + 90 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Calibration + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Place aircraft very flat, and then click level to compute the accelerometer and gyro bias + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + 500 + 16777215 + + + + Launch horizontal calibration. + + + Level + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + + 300 + 0 + + + + 0 + + + + + + + + + If enabled, a fast recalibration of gyro zero point will be done +whenever the frame is armed. Do not move the airframe while +arming it in that case! + + + Zero gyros while arming aircraft + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Filtering + + + + + + + 0 + 0 + + + + + + + Accelerometers + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 0 + + + + + + + + + 60 + 0 + + + + Accelerometer filtering. + +Sets the amount of lowpass filtering of accelerometer data +for the attitude estimation. Higher values apply a stronger +filter, which may help with drifting in attitude mode. + +Range: 0.00 - 0.20, Good starting value: 0.05 - 0.10 +Start low and raise until drift stops. + +A setting of 0.00 disables the filter. + + + 2 + + + 0.200000000000000 + + + 0.010000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Vertical + + + + 20 + 10 + + + + + + + + + + + + + + + + 4 + + + + + Qt::Horizontal + + + + 380 + 20 + + + + + + + + + 0 + 0 + + + + + 25 + 25 + + + + Takes you to the wiki page + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 25 + 25 + + + + true + + + + + + + + 0 + 0 + + + + Apply + + + + + + + + 0 + 0 + + + + Click to permanently save the accel bias in the CopterControl Flash. + + + Save + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 082c89cd5..004c10d1c 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -21,6 +21,8 @@ HEADERS += \ configinputwidget.h \ configoutputwidget.h \ configvehicletypewidget.h \ + config_cc_hw_widget.h \ + configccattitudewidget.h \ configstabilizationwidget.h \ assertions.h \ defaultattitudewidget.h \ @@ -67,6 +69,8 @@ SOURCES += \ configinputwidget.cpp \ configoutputwidget.cpp \ configvehicletypewidget.cpp \ + config_cc_hw_widget.cpp \ + configccattitudewidget.cpp \ configstabilizationwidget.cpp \ defaultattitudewidget.cpp \ defaulthwsettingswidget.cpp \ @@ -104,10 +108,12 @@ FORMS += \ airframe_ground.ui \ airframe_multirotor.ui \ airframe_custom.ui \ + cc_hw_settings.ui \ stabilization.ui \ input.ui \ input_wizard.ui \ output.ui \ + ccattitude.ui \ defaultattitude.ui \ defaulthwsettings.ui \ inputchannelform.ui \ diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp new file mode 100644 index 000000000..4fe823e6a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -0,0 +1,156 @@ +/** + ****************************************************************************** + * + * @file configtelemetrywidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to update settings in the firmware + *****************************************************************************/ +/* + * 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 "config_cc_hw_widget.h" +#include "hwsettings.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) +{ + m_telemetry = new Ui_CC_HW_Widget(); + m_telemetry->setupUi(this); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { + m_telemetry->saveTelemetryToRAM->setVisible(false); + } + + + UAVObjectUtilManager *utilMngr = pm->getObject(); + int id = utilMngr->getBoardModel(); + + switch (id) { + case 0x0101: + m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0101.svg")); + break; + case 0x0301: + m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0301.svg")); + break; + case 0x0401: + m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); + break; + case 0x0402: + m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); + break; + case 0x0201: + m_telemetry->label_2->setPixmap(QPixmap(":/uploader/images/deviceID-0201.svg")); + break; + default: + m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); + break; + } + addApplySaveButtons(m_telemetry->saveTelemetryToRAM, m_telemetry->saveTelemetryToSD); + addWidgetBinding("HwSettings", "CC_FlexiPort", m_telemetry->cbFlexi); + addWidgetBinding("HwSettings", "CC_MainPort", m_telemetry->cbTele); + addWidgetBinding("HwSettings", "CC_RcvrPort", m_telemetry->cbRcvr); + addWidgetBinding("HwSettings", "USB_HIDPort", m_telemetry->cbUsbHid); + addWidgetBinding("HwSettings", "USB_VCPPort", m_telemetry->cbUsbVcp); + addWidgetBinding("HwSettings", "TelemetrySpeed", m_telemetry->telemetrySpeed); + addWidgetBinding("HwSettings", "GPSSpeed", m_telemetry->gpsSpeed); + // Add Gps protocol configuration + + HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + HwSettings::DataFields hwSettingsData = hwSettings->getData(); + + if (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) { + m_telemetry->gpsProtocol->setEnabled(false); + m_telemetry->gpsProtocol->setToolTip(tr("Enable GPS module and reboot the board to be able to select GPS protocol")); + } else { + addWidgetBinding("GPSSettings", "DataProtocol", m_telemetry->gpsProtocol); + } + + addWidgetBinding("HwSettings", "ComUsbBridgeSpeed", m_telemetry->comUsbBridgeSpeed); + connect(m_telemetry->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + enableSaveButtons(false); + populateWidgets(); + refreshWidgetsValues(); + forceConnectedState(); +} + +ConfigCCHWWidget::~ConfigCCHWWidget() +{ + // Do nothing +} + +void ConfigCCHWWidget::refreshValues() +{} + +void ConfigCCHWWidget::widgetsContentsChanged() +{ + ConfigTaskWidget::widgetsContentsChanged(); + + if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && + (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE) && + (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_DEBUGCONSOLE) && + (m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE))) { + enableSaveButtons(false); + m_telemetry->problems->setText(tr("Warning: you have configured more than one DebugConsole, this currently is not supported")); + } else if (((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_TELEMETRY) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_TELEMETRY)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_GPS) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_GPS)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_DEBUGCONSOLE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_DEBUGCONSOLE)) || + ((m_telemetry->cbTele->currentIndex() == HwSettings::CC_MAINPORT_COMBRIDGE) && (m_telemetry->cbFlexi->currentIndex() == HwSettings::CC_FLEXIPORT_COMBRIDGE))) { + enableSaveButtons(false); + m_telemetry->problems->setText(tr("Warning: you have configured both MainPort and FlexiPort for the same function, this currently is not supported")); + } else if ((m_telemetry->cbUsbHid->currentIndex() == HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() == HwSettings::USB_VCPPORT_USBTELEMETRY)) { + enableSaveButtons(false); + m_telemetry->problems->setText(tr("Warning: you have configured both USB HID Port and USB VCP Port for the same function, this currently is not supported")); + } else if ((m_telemetry->cbUsbHid->currentIndex() != HwSettings::USB_HIDPORT_USBTELEMETRY) && (m_telemetry->cbUsbVcp->currentIndex() != HwSettings::USB_VCPPORT_USBTELEMETRY)) { + enableSaveButtons(false); + m_telemetry->problems->setText(tr("Warning: you have disabled USB Telemetry on both USB HID Port and USB VCP Port, this currently is not supported")); + } else { + m_telemetry->problems->setText(""); + enableSaveButtons(true); + } +} + +void ConfigCCHWWidget::enableSaveButtons(bool enable) +{ + m_telemetry->saveTelemetryToRAM->setEnabled(enable); + m_telemetry->saveTelemetryToSD->setEnabled(enable); +} + +void ConfigCCHWWidget::openHelp() +{ + QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/x/D4AUAQ"), QUrl::StrictMode)); +} + +/** + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h new file mode 100644 index 000000000..313231382 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * + * @file configtelemetrytwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Telemetry configuration panel + *****************************************************************************/ +/* + * 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 CONFIGCCHWWIDGET_H +#define CONFIGCCHWWIDGET_H + +#include "ui_cc_hw_settings.h" +#include "../uavobjectwidgetutils/configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include +#include "smartsavebutton.h" + +class ConfigCCHWWidget : public ConfigTaskWidget { + Q_OBJECT + +public: + ConfigCCHWWidget(QWidget *parent = 0); + ~ConfigCCHWWidget(); +private slots: + void openHelp(); + void refreshValues(); + void widgetsContentsChanged(); + void enableSaveButtons(bool enable); + +private: + Ui_CC_HW_Widget *m_telemetry; + QSvgRenderer *m_renderer; +}; + +#endif // CONFIGCCHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp new file mode 100644 index 000000000..4e872735b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -0,0 +1,239 @@ +/** + ****************************************************************************** + * + * @file configccattitudewidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Configure Attitude module on CopterControl + *****************************************************************************/ +/* + * 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 "configccattitudewidget.h" +#include "ui_ccattitude.h" +#include "utils/coordinateconversions.h" +#include "attitudesettings.h" +#include +#include +#include +#include +#include +#include "accelstate.h" +#include "accelgyrosettings.h" +#include "gyrostate.h" +#include +#include +#include +ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : + ConfigTaskWidget(parent), + ui(new Ui_ccattitude) +{ + ui->setupUi(this); + connect(ui->zeroBias, SIGNAL(clicked()), this, SLOT(startAccelCalibration())); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if (!settings->useExpertMode()) { + ui->applyButton->setVisible(false); + } + + addApplySaveButtons(ui->applyButton, ui->saveButton); + addUAVObject("AttitudeSettings"); + addUAVObject("AccelGyroSettings"); + + // Connect the help button + connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + + addWidgetBinding("AttitudeSettings", "ZeroDuringArming", ui->zeroGyroBiasOnArming); + addWidgetBinding("AttitudeSettings", "AccelTau", ui->accelTauSpinbox); + + addWidgetBinding("AttitudeSettings", "BoardRotation", ui->rollBias, AttitudeSettings::BOARDROTATION_ROLL); + addWidgetBinding("AttitudeSettings", "BoardRotation", ui->pitchBias, AttitudeSettings::BOARDROTATION_PITCH); + addWidgetBinding("AttitudeSettings", "BoardRotation", ui->yawBias, AttitudeSettings::BOARDROTATION_YAW); + addWidget(ui->zeroBias); + populateWidgets(); + refreshWidgetsValues(); + forceConnectedState(); +} + +ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() +{ + delete ui; +} + +void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) +{ + if (!timer.isActive()) { + // ignore updates that come in after the timer has expired + return; + } + + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + + // Accumulate samples until we have _at least_ NUM_SENSOR_UPDATES samples + // for both gyros and accels. + // Note that, at present, we stash the samples and then compute the bias + // at the end, even though the mean could be accumulated as we go. + // In future, a better algorithm could be used. + if (obj->getObjID() == AccelState::OBJID) { + accelUpdates++; + AccelState::DataFields accelStateData = accelState->getData(); + x_accum.append(accelStateData.x); + y_accum.append(accelStateData.y); + z_accum.append(accelStateData.z); + } else if (obj->getObjID() == GyroState::OBJID) { + gyroUpdates++; + GyroState::DataFields gyroStateData = gyroState->getData(); + x_gyro_accum.append(gyroStateData.x); + y_gyro_accum.append(gyroStateData.y); + z_gyro_accum.append(gyroStateData.z); + } + + // update the progress indicator + ui->zeroBiasProgress->setValue((float)qMin(accelUpdates, gyroUpdates) / NUM_SENSOR_UPDATES * 100); + + // If we have enough samples, then stop sampling and compute the biases + if (accelUpdates >= NUM_SENSOR_UPDATES && gyroUpdates >= NUM_SENSOR_UPDATES) { + timer.stop(); + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); + + float x_bias = OpenPilot::CalibrationUtils::listMean(x_accum); + float y_bias = OpenPilot::CalibrationUtils::listMean(y_accum); + float z_bias = OpenPilot::CalibrationUtils::listMean(z_accum) + 9.81; + + float x_gyro_bias = OpenPilot::CalibrationUtils::listMean(x_gyro_accum); + float y_gyro_bias = OpenPilot::CalibrationUtils::listMean(y_gyro_accum); + float z_gyro_bias = OpenPilot::CalibrationUtils::listMean(z_gyro_accum); + accelState->setMetadata(initialAccelStateMdata); + gyroState->setMetadata(initialGyroStateMdata); + + AccelGyroSettings::DataFields accelGyroSettingsData = AccelGyroSettings::GetInstance(getObjectManager())->getData(); + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); + // We offset the gyro bias by current bias to help precision + accelGyroSettingsData.accel_bias[0] += x_bias; + accelGyroSettingsData.accel_bias[1] += y_bias; + accelGyroSettingsData.accel_bias[2] += z_bias; + accelGyroSettingsData.gyro_bias[0] = -x_gyro_bias; + accelGyroSettingsData.gyro_bias[1] = -y_gyro_bias; + accelGyroSettingsData.gyro_bias[2] = -z_gyro_bias; + attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; + AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); + AccelGyroSettings::GetInstance(getObjectManager())->setData(accelGyroSettingsData); + this->setDirty(true); + + // reenable controls + enableControls(true); + } +} + +void ConfigCCAttitudeWidget::timeout() +{ + UAVDataObject *obj = AccelState::GetInstance(getObjectManager()); + + disconnect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + disconnect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); + + AccelState *accelState = AccelState::GetInstance(getObjectManager()); + GyroState *gyroState = GyroState::GetInstance(getObjectManager()); + accelState->setMetadata(initialAccelStateMdata); + gyroState->setMetadata(initialGyroStateMdata); + + QMessageBox msgBox; + msgBox.setText(tr("Calibration timed out before receiving required updates.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + + // reset progress indicator + ui->zeroBiasProgress->setValue(0); + // reenable controls + enableControls(true); +} + +void ConfigCCAttitudeWidget::startAccelCalibration() +{ + // disable controls during sampling + enableControls(false); + + accelUpdates = 0; + gyroUpdates = 0; + x_accum.clear(); + y_accum.clear(); + z_accum.clear(); + x_gyro_accum.clear(); + y_gyro_accum.clear(); + z_gyro_accum.clear(); + + // Disable gyro bias correction to see raw data + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); + attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; + AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); + + // Set up to receive updates + UAVDataObject *accelState = AccelState::GetInstance(getObjectManager()); + UAVDataObject *gyroState = GyroState::GetInstance(getObjectManager()); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sensorsUpdated(UAVObject *))); + + // Speed up updates + initialAccelStateMdata = accelState->getMetadata(); + UAVObject::Metadata accelStateMdata = initialAccelStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(accelStateMdata, UAVObject::UPDATEMODE_PERIODIC); + accelStateMdata.flightTelemetryUpdatePeriod = 30; // ms + accelState->setMetadata(accelStateMdata); + + initialGyroStateMdata = gyroState->getMetadata(); + UAVObject::Metadata gyroStateMdata = initialGyroStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(gyroStateMdata, UAVObject::UPDATEMODE_PERIODIC); + gyroStateMdata.flightTelemetryUpdatePeriod = 30; // ms + gyroState->setMetadata(gyroStateMdata); + + // Set up timeout timer + timer.setSingleShot(true); + timer.start(5000 + (NUM_SENSOR_UPDATES * qMax(accelStateMdata.flightTelemetryUpdatePeriod, + gyroStateMdata.flightTelemetryUpdatePeriod))); + connect(&timer, SIGNAL(timeout()), this, SLOT(timeout())); +} + +void ConfigCCAttitudeWidget::openHelp() +{ + QDesktopServices::openUrl(QUrl(tr("http://wiki.openpilot.org/x/44Cf"), QUrl::StrictMode)); +} + +void ConfigCCAttitudeWidget::setAccelFiltering(bool active) +{ + Q_UNUSED(active); + setDirty(true); +} + +void ConfigCCAttitudeWidget::enableControls(bool enable) +{ + ui->zeroBias->setEnabled(enable); + ui->zeroGyroBiasOnArming->setEnabled(enable); + ui->accelTauSpinbox->setEnabled(enable); + ConfigTaskWidget::enableControls(enable); +} + +void ConfigCCAttitudeWidget::updateObjectsFromWidgets() +{ + ConfigTaskWidget::updateObjectsFromWidgets(); + + ui->zeroBiasProgress->setValue(0); +} diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h new file mode 100644 index 000000000..b4d84a5b9 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -0,0 +1,74 @@ +/** + ****************************************************************************** + * + * @file configccattitudewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Configure the properties of the attitude module in CopterControl + *****************************************************************************/ +/* + * 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 CCATTITUDEWIDGET_H +#define CCATTITUDEWIDGET_H + +#include "ui_ccattitude.h" +#include "../uavobjectwidgetutils/configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include + +class Ui_Widget; + +class ConfigCCAttitudeWidget : public ConfigTaskWidget { + Q_OBJECT + +public: + explicit ConfigCCAttitudeWidget(QWidget *parent = 0); + ~ConfigCCAttitudeWidget(); + + virtual void updateObjectsFromWidgets(); + +private slots: + void sensorsUpdated(UAVObject *obj); + void timeout(); + void startAccelCalibration(); + void openHelp(); + void setAccelFiltering(bool active); + +private: + Ui_ccattitude *ui; + QTimer timer; + UAVObject::Metadata initialAccelStateMdata; + UAVObject::Metadata initialGyroStateMdata; + + int accelUpdates; + int gyroUpdates; + + QList x_accum, y_accum, z_accum; + QList x_gyro_accum, y_gyro_accum, z_gyro_accum; + + static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1; + static const int NUM_SENSOR_UPDATES = 300; +protected: + virtual void enableControls(bool enable); +}; + +#endif // CCATTITUDEWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index 6e96f1872..7767c9953 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -12,7 +12,9 @@ images/fixedwing-shapes.svg images/ground-shapes.svg images/ccpm_setup.svg + images/PipXtreme.png images/help.png + images/coptercontrol.svg images/TX2.svg images/output_selected.png images/output_normal.png diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index bb1a923f7..4782b767a 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -28,12 +28,14 @@ #include "configgadgetwidget.h" #include "configvehicletypewidget.h" +#include "configccattitudewidget.h" #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" #include "configcamerastabilizationwidget.h" #include "configtxpidwidget.h" #include "configrevohwwidget.h" +#include "config_cc_hw_widget.h" #include "configoplinkwidget.h" #include "configrevowidget.h" #include "configrevonanohwwidget.h" @@ -182,13 +184,18 @@ void ConfigGadgetWidget::onAutopilotConnect() UAVObjectUtilManager *utilMngr = pm->getObject(); if (utilMngr) { int board = utilMngr->getBoardModel(); - if ((board & 0xff00) == 0x0900) { + if ((board & 0xff00) == 0x0400) { + // CopterControl family + QWidget *qwd = new ConfigCCAttitudeWidget(this); + stackWidget->replaceTab(ConfigGadgetWidget::sensors, qwd); + qwd = new ConfigCCHWWidget(this); + stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd); + } else if ((board & 0xff00) == 0x0900) { // Revolution family QWidget *qwd = new ConfigRevoWidget(this); stackWidget->replaceTab(ConfigGadgetWidget::sensors, qwd); - if (board == 0x0903) { - qwd = new ConfigRevoHWWidget(this); - } else if (board == 0x0905) { + qwd = new ConfigRevoHWWidget(this); + if (board == 0x0905) { qwd = new ConfigRevoNanoHWWidget(this); } stackWidget->replaceTab(ConfigGadgetWidget::hardware, qwd); diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index a60c77dac..baeeb9877 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -354,7 +354,11 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj) if (utilMngr) { int board = utilMngr->getBoardModel(); // Setup labels and combos for banks according to board type - if (board == 0x0903) { + if ((board & 0xff00) == 0x0400) { + // Coptercontrol family of boards 4 timer banks + bankLabels << "1 (1-3)" << "2 (4)" << "3 (5,7-8)" << "4 (6,9-10)"; + channelBanks << 1 << 1 << 1 << 2 << 3 << 4 << 3 << 3 << 4 << 4; + } else if (board == 0x0903) { // Revolution family of boards 6 timer banks bankLabels << "1 (1-2)" << "2 (3)" << "3 (4)" << "4 (5-6)" << "5 (7,12)" << "6 (8-11)"; channelBanks << 1 << 1 << 2 << 3 << 4 << 4 << 5 << 6 << 6 << 6 << 6 << 5; @@ -472,7 +476,10 @@ void ConfigOutputWidget::updateWarnings(UAVObject *) if (systemAlarms.Alarm[SystemAlarms::ALARM_SYSTEMCONFIGURATION] > SystemAlarms::ALARM_WARNING) { switch (systemAlarms.ExtendedAlarmStatus[SystemAlarms::EXTENDEDALARMSTATUS_SYSTEMCONFIGURATION]) { case SystemAlarms::EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT: - setWarning(tr("OneShot and PWMSync output DO NOT work with Receiver Port is 'PWM'
")); + setWarning(tr("OneShot and PWMSync output only works with Receiver Port settings marked with '+OneShot'
" + "When using Receiver Port setting 'PPM_PIN8+OneShot' " + "Bank %2 must be set to PWM") + .arg(m_banks.at(3).color().name()).arg(m_banks.at(3).label()->text())); return; } } diff --git a/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png b/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png new file mode 100644 index 000000000..cd9396e6c Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/PipXtreme.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg b/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg new file mode 100644 index 000000000..669199ef6 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/images/coptercontrol.svg @@ -0,0 +1,2647 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index ed0599128..a225b97e6 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -89,8 +89,6 @@ void ConnectionManager::init() // new connection object from plugins QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject *)), this, SLOT(objectAdded(QObject *))); QObject::connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject *)), this, SLOT(aboutToRemoveObject(QObject *))); - - ccWarningClosed = true; } @@ -437,23 +435,16 @@ void ConnectionManager::devChanged(IConnection *connection) } // clear device list combobox m_availableDevList->clear(); - ccFound = false; // remove registered devices of this IConnection from the list updateConnectionList(connection); updateConnectionDropdown(); - if (ccFound && ccWarningClosed) { - ccWarningClosed = false; - QMessageBox::information(this, tr("CopterControl Not Supported"), - tr("This version of OpenPilot GCS does not support CC and CC3D boards.\n\nPlease use OpenPilot GCS version 15.02.xx instead")); - ccWarningClosed = true; - } - qDebug() << "# devices " << m_devList.count(); emit availableDevicesChanged(m_devList); + // disable connection button if the liNameif (m_availableDevList->count() > 0) if (m_availableDevList->count() > 0) { m_connectBtn->setEnabled(true); @@ -466,21 +457,17 @@ void ConnectionManager::updateConnectionDropdown() { // add all the list again to the combobox foreach(DevListItem d, m_devList) { - if (!d.getConName().contains("CopterControl")) { - m_availableDevList->addItem(d.getConName()); - m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); - if (!m_ioDev && d.getConName().startsWith("USB")) { - if (m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) { - m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); - } - if (m_mainWindow->generalSettings()->autoConnect() && polling) { - qDebug() << "Automatically opening device"; - connectDevice(d); - qDebug() << "ConnectionManager::updateConnectionDropdown autoconnected USB device"; - } + m_availableDevList->addItem(d.getConName()); + m_availableDevList->setItemData(m_availableDevList->count() - 1, d.getConName(), Qt::ToolTipRole); + if (!m_ioDev && d.getConName().startsWith("USB")) { + if (m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) { + m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); + } + if (m_mainWindow->generalSettings()->autoConnect() && polling) { + qDebug() << "Automatically opening device"; + connectDevice(d); + qDebug() << "ConnectionManager::updateConnectionDropdown autoconnected USB device"; } - } else { - ccFound = true; } } if (m_ioDev) { diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 1b133d831..b19cc7c5d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -38,7 +38,6 @@ #include #include #include -#include #include "core_global.h" #include @@ -150,8 +149,6 @@ protected: private: bool connectDevice(); bool polling; - bool ccFound; - bool ccWarningClosed; Internal::MainWindow *m_mainWindow; QList connectionBackup; QTimer *reconnect; diff --git a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h index ac806e33c..eda27387f 100644 --- a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h +++ b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h @@ -135,9 +135,10 @@ public: }; enum USBConstants { - idVendor_OpenPilot = 0x20a0, - idProduct_OpenPilot = 0x415a, - idProduct_OPLinkMini = 0x415c + idVendor_OpenPilot = 0x20a0, + idProduct_OpenPilot = 0x415a, + idProduct_CopterControl = 0x415b, + idProduct_OPLinkMini = 0x415c }; static USBMonitor *instance(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 619c22b63..c6f8b1d69 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -81,6 +81,10 @@ void ConnectionDiagram::setupGraphicsScene() QList elementsToShow; switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + elementsToShow << "controller-cc"; + break; case VehicleConfigurationSource::CONTROLLER_REVO: elementsToShow << "controller-revo"; break; @@ -160,6 +164,14 @@ void ConnectionDiagram::setupGraphicsScene() QString prefix = ""; QString suffix = ""; switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + prefix = "cc-"; + if (m_configSource->getEscType() == VehicleConfigurationSource::ESC_ONESHOT || + m_configSource->getEscType() == VehicleConfigurationSource::ESC_SYNCHED) { + suffix = "-oneshot"; + } + break; case VehicleConfigurationSource::CONTROLLER_REVO: prefix = "revo-"; break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp index 57396610a..3597e2cd9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -80,6 +80,9 @@ bool ControllerPage::isComplete() const bool ControllerPage::validatePage() { getWizard()->setControllerType((SetupWizard::CONTROLLER_TYPE)ui->boardTypeCombo->itemData(ui->boardTypeCombo->currentIndex()).toInt()); + if (getWizard()->getControllerType() == SetupWizard::CONTROLLER_CC || getWizard()->getControllerType() == SetupWizard::CONTROLLER_CC3D) { + getWizard()->setGpsType(SetupWizard::GPS_DISABLED); + } return true; } @@ -98,6 +101,12 @@ SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() case 0x0301: return SetupWizard::CONTROLLER_OPLINK; + case 0x0401: + return SetupWizard::CONTROLLER_CC; + + case 0x0402: + return SetupWizard::CONTROLLER_CC3D; + case 0x0903: return SetupWizard::CONTROLLER_REVO; @@ -123,6 +132,8 @@ void ControllerPage::setupBoardTypes() QVariant v(0); ui->boardTypeCombo->addItem(tr(""), SetupWizard::CONTROLLER_UNKNOWN); + ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl"), SetupWizard::CONTROLLER_CC); + ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl 3D"), SetupWizard::CONTROLLER_CC3D); ui->boardTypeCombo->addItem(tr("OpenPilot Revolution"), SetupWizard::CONTROLLER_REVO); ui->boardTypeCombo->addItem(tr("OpenPilot OPLink Radio Modem"), SetupWizard::CONTROLLER_OPLINK); ui->boardTypeCombo->addItem(tr("OpenPilot DiscoveryF4"), SetupWizard::CONTROLLER_DISCOVERYF4); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp index 245ed42f0..8b84ec861 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp @@ -77,9 +77,10 @@ bool EscPage::isSynchOrOneShotAvailable() switch (getWizard()->getControllerType()) { case SetupWizard::CONTROLLER_NANO: + case SetupWizard::CONTROLLER_CC: + case SetupWizard::CONTROLLER_CC3D: available = getWizard()->getInputType() != SetupWizard::INPUT_PWM; break; - default: break; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index e5b752cdf..25ab2000a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -74,6 +74,27 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp HwSettings *hwSettings = HwSettings::GetInstance(uavoManager); HwSettings::DataFields data = hwSettings->getData(); switch (getWizard()->getControllerType()) { + case SetupWizard::CONTROLLER_CC: + case SetupWizard::CONTROLLER_CC3D: + { + switch (selectedType) { + case VehicleConfigurationSource::INPUT_PWM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWMNOONESHOT; + + case VehicleConfigurationSource::INPUT_PPM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPMNOONESHOT; + + case VehicleConfigurationSource::INPUT_SBUS: + return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; + + case VehicleConfigurationSource::INPUT_DSM: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM; + + default: return true; + } + break; + } case SetupWizard::CONTROLLER_REVO: case SetupWizard::CONTROLLER_DISCOVERYF4: case SetupWizard::CONTROLLER_NANO: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 4707cc47f..28b33f15f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -95,6 +95,8 @@ int SetupWizard::nextId() const case PAGE_CONTROLLER: { switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: case CONTROLLER_REVO: case CONTROLLER_DISCOVERYF4: return PAGE_INPUT; @@ -207,6 +209,8 @@ int SetupWizard::nextId() const case PAGE_SUMMARY: { switch (getControllerType()) { + case CONTROLLER_CC: + case CONTROLLER_CC3D: case CONTROLLER_REVO: case CONTROLLER_NANO: case CONTROLLER_DISCOVERYF4: @@ -238,6 +242,12 @@ QString SetupWizard::getSummaryText() summary.append("").append(tr("Controller type: ")).append(""); switch (getControllerType()) { + case CONTROLLER_CC: + summary.append(tr("OpenPilot CopterControl")); + break; + case CONTROLLER_CC3D: + summary.append(tr("OpenPilot CopterControl 3D")); + break; case CONTROLLER_REVO: summary.append(tr("OpenPilot Revolution")); break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 0546152c4..4e330847c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -100,9 +100,16 @@ bool VehicleConfigurationHelper::setupHardwareSettings(bool save) bool VehicleConfigurationHelper::isApplicable(UAVObject *dataObj) { - Q_UNUSED(dataObj); + switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + if (dataObj->getName() == "EKFConfiguration") { + return false; + } + default: return true; } +} void VehicleConfigurationHelper::addModifiedObject(UAVDataObject *object, QString description) { @@ -129,6 +136,39 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() data.OptionalModules[HwSettings::OPTIONALMODULES_AIRSPEED] = 0; switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + // Reset all ports + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLEDONESHOT; + + // Default mainport to be active telemetry link + data.CC_MainPort = HwSettings::CC_MAINPORT_TELEMETRY; + + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DISABLED; + switch (m_configSource->getInputType()) { + case VehicleConfigurationSource::INPUT_PWM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWMNOONESHOT; + break; + case VehicleConfigurationSource::INPUT_PPM: + if (m_configSource->getEscType() == VehicleConfigurationSource::ESC_ONESHOT || + m_configSource->getEscType() == VehicleConfigurationSource::ESC_SYNCHED) { + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM_PIN8ONESHOT; + } else { + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPMNOONESHOT; + } + break; + case VehicleConfigurationSource::INPUT_SBUS: + // We have to set teletry on flexport since s.bus needs the mainport. + data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; + break; + case VehicleConfigurationSource::INPUT_DSM: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM; + break; + default: + break; + } + break; case VehicleConfigurationSource::CONTROLLER_REVO: case VehicleConfigurationSource::CONTROLLER_NANO: case VehicleConfigurationSource::CONTROLLER_DISCOVERYF4: @@ -597,6 +637,20 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings")); switch (m_configSource->getControllerType()) { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + { + AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); + Q_ASSERT(copterControlCalibration); + AttitudeSettings::DataFields data = copterControlCalibration->getData(); + + data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU; + data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; + + copterControlCalibration->setData(data); + addModifiedObject(copterControlCalibration, tr("Writing board settings")); + break; + } case VehicleConfigurationSource::CONTROLLER_REVO: case VehicleConfigurationSource::CONTROLLER_NANO: { diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 07a769e5c..44e2a8d81 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -57,7 +57,7 @@ class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4 }; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_OPLINK, CONTROLLER_DISCOVERYF4 }; 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/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index 23a162fe1..e2760a219 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -14,18 +14,43 @@ public: static QString idToBoardName(int id) { switch (id) { + case 0x0101: + // MB + return QString("OpenPilot MainBoard"); + + break; + case 0x0201: + // INS + return QString("OpenPilot INS"); + + break; case 0x0301: // OPLink Mini return QString("OPLink"); + break; + case 0x0401: + // Coptercontrol + return QString("CopterControl"); + + break; + case 0x0402: + // Coptercontrol 3D + // It would be nice to say CC3D here but since currently we use string comparisons + // for firmware compatibility and the filename path that would break + return QString("CopterControl"); + + break; case 0x0901: // Revolution return QString("Revolution"); + break; case 0x0903: // Revo Mini return QString("Revolution"); + break; case 0x0904: return QString("DiscoveryF4"); @@ -36,6 +61,8 @@ public: default: return QString(""); + + break; } } deviceDescriptorStruct() {} diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index af120d9b3..c99bd286d 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -78,11 +78,6 @@ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; - // Exclude CC/CC3D - if ((id == 0x0401) || (id == 0x0402)) { - return; - } - myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: @@ -91,10 +86,22 @@ void DeviceWidget::populate() myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16)); switch (id) { + case 0x0101: + case 0x0201: + devicePic.load(""); + break; case 0x0301: devicePic.load(":/uploader/images/gcs-board-oplink.png"); break; + case 0x0401: + devicePic.load(":/uploader/images/gcs-board-cc.png"); + break; + case 0x0402: + devicePic.load(":/uploader/images/gcs-board-cc3d.png"); + break; case 0x0903: + devicePic.load(":/uploader/images/gcs-board-revo.png"); + break; case 0x0904: devicePic.load(":/uploader/images/gcs-board-revo.png"); break; @@ -357,7 +364,8 @@ void DeviceWidget::uploadFirmware() // - Check whether board type matches firmware: int board = m_dfu->devices[deviceID].ID; int firmwareBoard = ((desc.at(12) & 0xff) << 8) + (desc.at(13) & 0xff); - if ((board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware + if ((board == 0x401 && firmwareBoard == 0x402) || + (board == 0x901 && firmwareBoard == 0x902) || // L3GD20 revo supports Revolution firmware (board == 0x902 && firmwareBoard == 0x903)) { // RevoMini1 supporetd by RevoMini2 firmware // These firmwares are designed to be backwards compatible } else if (firmwareBoard != board) { diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg new file mode 100644 index 000000000..adc9a3d05 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0101.svg @@ -0,0 +1,2466 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg new file mode 100644 index 000000000..3b6a15ebb --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0201.svg @@ -0,0 +1,2948 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg new file mode 100644 index 000000000..be0166618 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0301.svg @@ -0,0 +1,61 @@ + + + + + + + + image/svg+xml + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg new file mode 100644 index 000000000..d18741f4a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0401.svg @@ -0,0 +1,2646 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg new file mode 100644 index 000000000..90121943d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/deviceID-0402.svg @@ -0,0 +1,2389 @@ + + + +image/svg+xmlCC3D + \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png new file mode 100644 index 000000000..ee6affc3c Binary files /dev/null and b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png differ diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png new file mode 100644 index 000000000..24326c952 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png differ diff --git a/ground/openpilotgcs/src/plugins/uploader/images/pipx.png b/ground/openpilotgcs/src/plugins/uploader/images/pipx.png new file mode 100644 index 000000000..47c8bb4e1 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/uploader/images/pipx.png differ diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index b516149f7..58401b8a0 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -1074,6 +1074,7 @@ int DFUObject::receiveData(void *data, int size) #define BOARD_ID_MB 1 #define BOARD_ID_INS 2 #define BOARD_ID_PIP 3 +#define BOARD_ID_CC 4 #define BOARD_ID_REVO 9 /** @@ -1097,6 +1098,9 @@ OP_DFU::eBoardType DFUObject::GetBoardType(int boardNum) case BOARD_ID_PIP: // PIP RF Modem brdType = eBoardPip; break; + case BOARD_ID_CC: // CopterControl family + brdType = eBoardCC; + break; case BOARD_ID_REVO: // Revo board brdType = eBoardRevo; break; diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h index 4409bc117..346582872 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.h +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.h @@ -92,6 +92,7 @@ enum eBoardType { eBoardMainbrd = 1, eBoardINS, eBoardPip = 3, + eBoardCC = 4, eBoardRevo = 9, }; diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index fae4ed879..aa5caafcb 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -72,9 +72,19 @@ void RunningDeviceWidget::populate() myDevice->devicePicture->scene()->clear(); switch (id) { + case 0x0101: + case 0x0201: + devicePic.load(""); + break; case 0x0301: devicePic.load(":/uploader/images/gcs-board-oplink.png"); break; + case 0x0401: + devicePic.load(":/uploader/images/gcs-board-cc.png"); + break; + case 0x0402: + devicePic.load(":/uploader/images/gcs-board-cc3d.png"); + break; case 0x0903: devicePic.load(":/uploader/images/gcs-board-revo.png"); break; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc index 240b06dc8..642cca091 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc @@ -5,9 +5,17 @@ images/process-stop.svg images/dialog-apply.svg images/gtk-info.svg + images/deviceID-0401.svg + images/deviceID-0301.svg + images/deviceID-0201.svg + images/deviceID-0101.svg images/application-certificate.svg images/warning.svg images/error.svg + images/deviceID-0402.svg + images/gcs-board-cc.png + images/gcs-board-cc3d.png + images/pipx.png images/gcs-board-oplink.png images/gcs-board-revo.png images/gcs-board-nano.png diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index e218704b1..2897b7e2c 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -752,6 +752,10 @@ bool UploaderGadgetWidget::autoUpdate(bool erase) case 0x301: filename = "fw_oplinkmini"; break; + case 0x401: + case 0x402: + filename = "fw_coptercontrol"; + break; case 0x501: filename = "fw_osd"; break; @@ -813,6 +817,7 @@ bool UploaderGadgetWidget::autoUpdate(bool erase) commonSystemBoot(false, erase); // Wait for board to connect to GCS again after boot and erase + // For older board like CC3D this can take some time // Theres a special case with OPLink if (!telemetryManager->isConnected() && !m_oplinkwatchdog.isConnected()) { progressUpdate(erase ? BOOTING_AND_ERASING : BOOTING, QVariant()); diff --git a/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp b/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp index 77349d23e..c561ab41f 100644 --- a/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp +++ b/ground/openpilotgcs/src/plugins/usagetracker/usagetrackerplugin.cpp @@ -182,11 +182,18 @@ void UsageTrackerPlugin::collectUsageParameters(QMap ¶mete parameters["conf_receiver"] = getUAVFieldValue(objManager, "ManualControlSettings", "ChannelGroups", 0); parameters["conf_vehicle"] = getUAVFieldValue(objManager, "SystemSettings", "AirframeType"); - // Revolution family - parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "RM_RcvrPort"); - parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "RM_MainPort"); - parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "RM_FlexiPort"); - parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm"); + if ((boardModel & 0xff00) == 0x0400) { + // CopterControl family + parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "CC_RcvrPort"); + parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "CC_MainPort"); + parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "CC_FlexiPort"); + } else if ((boardModel & 0xff00) == 0x0900) { + // Revolution family + parameters["conf_rport"] = getUAVFieldValue(objManager, "HwSettings", "RM_RcvrPort"); + parameters["conf_mport"] = getUAVFieldValue(objManager, "HwSettings", "RM_MainPort"); + parameters["conf_fport"] = getUAVFieldValue(objManager, "HwSettings", "RM_FlexiPort"); + parameters["conf_fusion"] = getUAVFieldValue(objManager, "RevoSettings", "FusionAlgorithm"); + } parameters["conf_uport"] = getUAVFieldValue(objManager, "HwSettings", "USB_HIDPort"); parameters["conf_vport"] = getUAVFieldValue(objManager, "HwSettings", "USB_VCPPort"); diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 83585105a..fc412bd49 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,6 +1,10 @@ Selection of optional hardware configurations. + + + +