diff --git a/HISTORY.txt b/HISTORY.txt index 45c119602..ac278b889 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,21 +1,30 @@ -Short summary of changes. For a complete list see the git log. - -2011-07-17 -Updated module initialization from Mathieu which separates the initialization -from the task startup. Also implements a method to reclaim unused ram from -initialization and end of memory for the FreeRTOS heap. - -2011-07-12 -Improvements to the stabilization code. Included a LPF on the gyros to smooth -out noise in high vibration environments. Also two new modes: axis-lock and -weak leveling. Axis-lock will try and hold an axis at a fixed position and -reject any disturbances. This is like heading-hold on a heli for the tail but -can be useful for other axes. Weak leveling is rate mode with a weak -correction to self level the craft - good for easier rate mode flying. - -2011-07-07 -Dynamic hardware configuration from Stac. The input type is now -selected from ManualControlSettings.InputMode and the aircraft must be rebooted -after changing this. Also for CopterControl the HwSettings object must -indicate which modules are connected to which ports. PPM currently not -working. +Short summary of changes. For a complete list see the git log. + +2011-08-04 +Fixed packaging aesthetic issues. Also avoid runtime issues on OSX Lion by +disabling the ModelView and Notify plugins for now (sorry). + +2011-07-29 +Added support for PPM receivers from James W. Now all 4 interfaces (R/C +standard PWM, combined PPM (MK), Spektrum satellite, Futaba S.Bus) are +supported and configurable through the GCS hardware configuration tab. + +2011-07-17 +Updated module initialization from Mathieu which separates the initialization +from the task startup. Also implements a method to reclaim unused ram from +initialization and end of memory for the FreeRTOS heap. + +2011-07-12 +Improvements to the stabilization code. Included a LPF on the gyros to smooth +out noise in high vibration environments. Also two new modes: axis-lock and +weak leveling. Axis-lock will try and hold an axis at a fixed position and +reject any disturbances. This is like heading-hold on a heli for the tail but +can be useful for other axes. Weak leveling is rate mode with a weak +correction to self level the craft - good for easier rate mode flying. + +2011-07-07 +Dynamic hardware configuration from Stac. The input type is now +selected from ManualControlSettings.InputMode and the aircraft must be rebooted +after changing this. Also for CopterControl the HwSettings object must +indicate which modules are connected to which ports. PPM currently not +working. diff --git a/flight/AHRS/pios_board.c b/flight/AHRS/pios_board.c index 2e5fd4430..aa5b20855 100644 --- a/flight/AHRS/pios_board.c +++ b/flight/AHRS/pios_board.c @@ -54,7 +54,6 @@ static const struct pios_spi_cfg pios_spi_op_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), @@ -157,7 +156,6 @@ static const struct pios_adc_cfg pios_adc_cfg = { .dma = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .init = { .NVIC_IRQChannel = DMA1_Channel1_IRQn, @@ -217,7 +215,6 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -249,6 +246,9 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { #include +#define PIOS_COM_AUX_TX_BUF_LEN 192 +static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; + #endif /* PIOS_INCLUDE_COM */ #if defined(PIOS_INCLUDE_I2C) @@ -294,7 +294,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .event = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C1_EV_IRQn, @@ -304,7 +303,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .error = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C1_ER_IRQn, @@ -375,12 +373,16 @@ void PIOS_Board_Init(void) { /* Communication system */ #if !defined(PIOS_ENABLE_DEBUG_PINS) #if defined(PIOS_INCLUDE_COM) - uint32_t pios_usart_aux_id; - if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id)) { - PIOS_DEBUG_Assert(0); + { + uint32_t pios_usart_aux_id; + if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, + NULL, 0, + pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) { + PIOS_DEBUG_Assert(0); + } } #endif /* PIOS_INCLUDE_COM */ #endif diff --git a/flight/Bootloaders/AHRS/pios_board.c b/flight/Bootloaders/AHRS/pios_board.c index fc3774214..756da9288 100644 --- a/flight/Bootloaders/AHRS/pios_board.c +++ b/flight/Bootloaders/AHRS/pios_board.c @@ -55,7 +55,6 @@ static const struct pios_spi_cfg .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .init = { .NVIC_IRQChannel = DMA1_Channel4_IRQn, @@ -133,11 +132,18 @@ void PIOS_SPI_op_irq_handler(void) { #include "bl_fsm.h" /* lfsm_* */ +static bool board_init_complete = false; void PIOS_Board_Init() { + if (board_init_complete) { + return; + } + /* Set up the SPI interface to the OP board */ if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { PIOS_DEBUG_Assert(0); } lfsm_attach(pios_spi_op_id); lfsm_init(); + + board_init_complete = true; } diff --git a/flight/Bootloaders/CopterControl/Makefile b/flight/Bootloaders/CopterControl/Makefile index 4fc2f0f08..fe613f909 100644 --- a/flight/Bootloaders/CopterControl/Makefile +++ b/flight/Bootloaders/CopterControl/Makefile @@ -94,8 +94,6 @@ DOXYGENDIR = ../Doc/Doxygen SRC += $(OPSYSTEM)/main.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/op_dfu.c -SRC += $(FLIGHTLIB)/stopwatch.c - ## PIOS Hardware (STM32F10x) SRC += $(PIOSSTM32F10X)/pios_sys.c @@ -139,7 +137,6 @@ SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c -SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c SRC += $(STMSPDSRCDIR)/misc.c diff --git a/flight/Bootloaders/CopterControl/main.c b/flight/Bootloaders/CopterControl/main.c index 67f6a4c32..aa375a49c 100644 --- a/flight/Bootloaders/CopterControl/main.c +++ b/flight/Bootloaders/CopterControl/main.c @@ -28,7 +28,6 @@ /* Bootloader Includes */ #include #include -#include "stopwatch.h" #include "op_dfu.h" #include "usb_lib.h" #include "pios_iap.h" @@ -47,9 +46,9 @@ pFunction Jump_To_Application; uint32_t JumpAddress; /// LEDs PWM -uint32_t period1 = 50; // *100 uS -> 5 mS +uint32_t period1 = 5000; // 5 mS uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS -uint32_t period2 = 50; // *100 uS -> 5 mS +uint32_t period2 = 5000; // 5 mS uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS @@ -70,7 +69,6 @@ uint8_t processRX(); void jump_to_app(); #define BLUE LED1 -#define LED_PWM_TIMER TIM1 int main() { PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) @@ -93,13 +91,17 @@ int main() { DeviceState = DFUidle; else DeviceState = BLidle; - STOPWATCH_Init(100, LED_PWM_TIMER); } else JumpToApp = TRUE; - STOPWATCH_Reset(LED_PWM_TIMER); - + 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(); @@ -107,19 +109,19 @@ int main() { case Last_operation_Success: case uploadingStarting: case DFUidle: - period1 = 50; + period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(BLUE); period2 = 0; break; case uploading: - period1 = 50; + period1 = 5000; sweep_steps1 = 100; - period2 = 25; + period2 = 2500; sweep_steps2 = 50; break; case downloading: - period1 = 25; + period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(BLUE); period2 = 0; @@ -130,14 +132,14 @@ int main() { period2 = 0; break; default://error - period1 = 50; + period1 = 5000; sweep_steps1 = 100; - period2 = 50; + period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) + if (LedPWM(period1, sweep_steps1, stopwatch)) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); @@ -145,16 +147,16 @@ int main() { PIOS_LED_On(BLUE); if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) + if (LedPWM(period2, sweep_steps2, stopwatch)) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_Off(BLUE); - if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) - STOPWATCH_Reset(LED_PWM_TIMER); - if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState + if (stopwatch > 50 * 1000 * 1000) + stopwatch = 0; + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState == BLidle)) JumpToApp = TRUE; @@ -185,20 +187,21 @@ void jump_to_app() { } } uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps) - / (pwm_sweep_steps / pwm_period); - if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period - * pwm_sweep_steps) - pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks + 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() { while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) { - for (int32_t x = 0; x < 63; ++x) { - mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB); + if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + processComand(mReceive_Buffer); } - processComand(mReceive_Buffer); } return TRUE; } diff --git a/flight/Bootloaders/CopterControl/pios_board.c b/flight/Bootloaders/CopterControl/pios_board.c index 2277080d8..28a280f4b 100644 --- a/flight/Bootloaders/CopterControl/pios_board.c +++ b/flight/Bootloaders/CopterControl/pios_board.c @@ -31,10 +31,31 @@ #include +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + +static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; +static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; + #endif /* PIOS_INCLUDE_COM */ // *********************************************************************************** +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_usb_id; @@ -44,7 +65,12 @@ uint32_t pios_com_telem_usb_id; * 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); @@ -58,13 +84,20 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg)) { + PIOS_Assert(0); + } #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { - PIOS_DEBUG_Assert(0); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id, + pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), + pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_COM */ -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + + board_init_complete = true; } diff --git a/flight/Bootloaders/OpenPilot/main.c b/flight/Bootloaders/OpenPilot/main.c index 3ed5a80aa..8e15952ea 100644 --- a/flight/Bootloaders/OpenPilot/main.c +++ b/flight/Bootloaders/OpenPilot/main.c @@ -250,10 +250,9 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { uint8_t processRX() { if (ProgPort == Usb) { while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) { - for (int32_t x = 0; x < 63; ++x) { - mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB); + if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + processComand(mReceive_Buffer); } - processComand(mReceive_Buffer); } } else if (ProgPort == Serial) { @@ -279,7 +278,12 @@ void SSP_CallBack(uint8_t *buf, uint16_t len) { } int16_t SSP_SerialRead(void) { if (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF) > 0) { - return PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF); + uint8_t byte; + if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) { + return byte; + } else { + return -1; + } } else return -1; } diff --git a/flight/Bootloaders/OpenPilot/pios_board.c b/flight/Bootloaders/OpenPilot/pios_board.c index d1727f6f6..5defbb626 100644 --- a/flight/Bootloaders/OpenPilot/pios_board.c +++ b/flight/Bootloaders/OpenPilot/pios_board.c @@ -60,7 +60,6 @@ const struct pios_spi_cfg .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .init = { .NVIC_IRQChannel = DMA1_Channel4_IRQn, @@ -157,7 +156,6 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .USART_HardwareFlowControl = USART_HardwareFlowControl_None, .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART2_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -186,8 +184,35 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { #include "pios_com_priv.h" +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + +static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; +static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; + +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 + +static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN]; +static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN]; + #endif /* PIOS_INCLUDE_COM */ +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_rf_id; @@ -200,7 +225,11 @@ uint32_t pios_com_telem_usb_id; * 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); @@ -218,7 +247,9 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, - pios_usart_telem_rf_id)) { + pios_usart_telem_rf_id, + pios_com_telem_rf_rx_buffer, sizeof(pios_com_telem_rf_rx_buffer), + pios_com_telem_rf_tx_buffer, sizeof(pios_com_telem_rf_tx_buffer))) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_COM */ @@ -226,13 +257,16 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { - PIOS_DEBUG_Assert(0); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id, + pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), + pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_COM */ -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar @@ -243,6 +277,8 @@ void PIOS_Board_Init(void) { /* Bind the AHRS comms layer to the AHRS SPI link */ PIOS_OPAHRS_Attach(pios_spi_ahrs_id); + + board_init_complete = true; } /** diff --git a/flight/Bootloaders/PipXtreme/Makefile b/flight/Bootloaders/PipXtreme/Makefile index dbf537bab..64f13d0d2 100644 --- a/flight/Bootloaders/PipXtreme/Makefile +++ b/flight/Bootloaders/PipXtreme/Makefile @@ -94,8 +94,6 @@ DOXYGENDIR = ../Doc/Doxygen SRC += $(OPSYSTEM)/main.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/op_dfu.c -SRC += $(FLIGHTLIB)/stopwatch.c - ## PIOS Hardware (STM32F10x) SRC += $(PIOSSTM32F10X)/pios_sys.c @@ -138,7 +136,6 @@ SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c -SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c SRC += $(STMSPDSRCDIR)/misc.c diff --git a/flight/Bootloaders/PipXtreme/main.c b/flight/Bootloaders/PipXtreme/main.c index d1568d163..3f1c4bd54 100644 --- a/flight/Bootloaders/PipXtreme/main.c +++ b/flight/Bootloaders/PipXtreme/main.c @@ -28,7 +28,6 @@ /* Bootloader Includes */ #include #include -#include "stopwatch.h" #include "op_dfu.h" #include "usb_lib.h" #include "pios_iap.h" @@ -47,9 +46,9 @@ pFunction Jump_To_Application; uint32_t JumpAddress; /// LEDs PWM -uint32_t period1 = 50; // *100 uS -> 5 mS +uint32_t period1 = 5000; // 5 mS uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS -uint32_t period2 = 50; // *100 uS -> 5 mS +uint32_t period2 = 5000; // 5 mS uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS @@ -71,7 +70,6 @@ void jump_to_app(); #define BLUE LED1 #define RED LED4 -#define LED_PWM_TIMER TIM3 int main() { /* NOTE: Do NOT modify the following start-up sequence */ /* Any new initialization functions should be added in OpenPilotInit() */ @@ -98,35 +96,37 @@ int main() { DeviceState = DFUidle; else DeviceState = BLidle; - STOPWATCH_Init(100, LED_PWM_TIMER); } else JumpToApp = TRUE; - STOPWATCH_Reset(LED_PWM_TIMER); - + 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(); - //pwm_period = 50; // *100 uS -> 5 mS - //pwm_sweep_steps =100; // * 5 mS -> 500 mS switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: - period1 = 50; + period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(RED); period2 = 0; break; case uploading: - period1 = 50; + period1 = 5000; sweep_steps1 = 100; - period2 = 25; + period2 = 2500; sweep_steps2 = 50; break; case downloading: - period1 = 25; + period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(RED); period2 = 0; @@ -137,14 +137,14 @@ int main() { period2 = 0; break; default://error - period1 = 50; + period1 = 5000; sweep_steps1 = 100; - period2 = 50; + period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { - if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) + if (LedPWM(period1, sweep_steps1, stopwatch)) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); @@ -152,16 +152,16 @@ int main() { PIOS_LED_On(BLUE); if (period2 != 0) { - if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) + if (LedPWM(period2, sweep_steps2, stopwatch)) PIOS_LED_On(RED); else PIOS_LED_Off(RED); } else PIOS_LED_Off(RED); - if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) - STOPWATCH_Reset(LED_PWM_TIMER); - if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState + if (stopwatch > 50 * 1000 * 1000) + stopwatch = 0; + if ((stopwatch > 6 * 1000 * 1000) && (DeviceState == BLidle)) JumpToApp = TRUE; @@ -193,20 +193,21 @@ void jump_to_app() { } } uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { - uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps) - / (pwm_sweep_steps / pwm_period); - if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period - * pwm_sweep_steps) - pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks + 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() { while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) { - for (int32_t x = 0; x < 63; ++x) { - mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB); + if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + processComand(mReceive_Buffer); } - processComand(mReceive_Buffer); } return TRUE; } diff --git a/flight/Bootloaders/PipXtreme/pios_board.c b/flight/Bootloaders/PipXtreme/pios_board.c index 2277080d8..83f0f407a 100644 --- a/flight/Bootloaders/PipXtreme/pios_board.c +++ b/flight/Bootloaders/PipXtreme/pios_board.c @@ -31,10 +31,31 @@ #include +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + +static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; +static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; + #endif /* PIOS_INCLUDE_COM */ // *********************************************************************************** +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_usb_id; @@ -44,7 +65,12 @@ uint32_t pios_com_telem_usb_id; * 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); @@ -58,13 +84,18 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { - PIOS_DEBUG_Assert(0); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id, + pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), + pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_COM */ -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar + + board_init_complete = true; } diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 2fa2c9116..ee3deaf5c 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -63,7 +63,6 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .init = { .NVIC_IRQChannel = DMA1_Channel4_IRQn, @@ -156,7 +155,6 @@ static const struct pios_adc_cfg pios_adc_cfg = { .dma = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .init = { .NVIC_IRQChannel = DMA1_Channel1_IRQn, @@ -216,7 +214,6 @@ static const struct pios_usart_cfg pios_usart_telem_main_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -253,7 +250,6 @@ static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -295,7 +291,6 @@ static const struct pios_usart_cfg pios_usart_gps_main_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -332,7 +327,6 @@ static const struct pios_usart_cfg pios_usart_gps_flexi_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -376,7 +370,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = { .USART_Mode = USART_Mode_Rx, }, .irq = { - .handler = PIOS_SPEKTRUM_irq_handler, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -425,7 +418,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = { .USART_Mode = USART_Mode_Rx, }, .irq = { - .handler = PIOS_SPEKTRUM_irq_handler, .init = { .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -482,7 +474,6 @@ static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { .USART_Mode = USART_Mode_Rx, }, .irq = { - .handler = PIOS_SBUS_irq_handler, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -531,6 +522,14 @@ static const struct pios_sbus_cfg pios_sbus_cfg = { #include "pios_com_priv.h" +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 + +#define PIOS_COM_GPS_RX_BUF_LEN 96 + +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + #endif /* PIOS_INCLUDE_COM */ #if defined(PIOS_INCLUDE_RTC) @@ -545,7 +544,6 @@ static const struct pios_rtc_cfg pios_rtc_main_cfg = { .clksrc = RCC_RTCCLKSource_HSE_Div128, .prescaler = 100, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = RTC_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -666,7 +664,6 @@ const struct pios_ppm_cfg pios_ppm_cfg = { }, .remap = 0, .irq = { - .handler = TIM4_IRQHandler, .init = { .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, @@ -761,7 +758,6 @@ const struct pios_pwm_cfg pios_pwm_cfg = { }, .remap = 0, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, @@ -826,7 +822,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .event = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_EV_IRQn, @@ -836,7 +831,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .error = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_ER_IRQn, @@ -869,6 +863,21 @@ struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS] uint32_t pios_rcvr_max_channel; #endif /* PIOS_INCLUDE_RCVR */ +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_rf_id; @@ -925,7 +934,14 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { + + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } @@ -934,12 +950,15 @@ void PIOS_Board_Init(void) { case HWSETTINGS_CC_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { - PIOS_SBUS_Init(&pios_sbus_cfg); - 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); + } } #endif /* PIOS_INCLUDE_SBUS */ break; @@ -950,7 +969,12 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_main_cfg)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { + + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { PIOS_Assert(0); } } @@ -959,13 +983,15 @@ void PIOS_Board_Init(void) { case HWSETTINGS_CC_MAINPORT_SPEKTRUM: #if defined(PIOS_INCLUDE_SPEKTRUM) { - /* SPEKTRUM init must come before usart init since it may use Rx pin for bind */ - PIOS_SPEKTRUM_Init(&pios_spektrum_main_cfg, false); - uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_id; + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + PIOS_Assert(0); + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -987,7 +1013,13 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_flexi_cfg)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } @@ -1000,7 +1032,11 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_flexi_cfg)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { PIOS_Assert(0); } } @@ -1009,13 +1045,15 @@ void PIOS_Board_Init(void) { case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM: #if defined(PIOS_INCLUDE_SPEKTRUM) { - /* SPEKTRUM init must come before usart init since it may use Rx pin for bind */ - PIOS_SPEKTRUM_Init(&pios_spektrum_flexi_cfg, false); - uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_flexi_cfg)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_id; + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + PIOS_Assert(0); + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1042,7 +1080,7 @@ void PIOS_Board_Init(void) { PIOS_PWM_Init(); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { - PIOS_Assert(0); + PIOS_Assert(0); } for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); @@ -1058,7 +1096,7 @@ void PIOS_Board_Init(void) { PIOS_PPM_Init(); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { - PIOS_Assert(0); + PIOS_Assert(0); } for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); @@ -1114,9 +1152,16 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_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 */ diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c index 239aa258b..ffbf27c45 100644 --- a/flight/INS/pios_board.c +++ b/flight/INS/pios_board.c @@ -61,7 +61,6 @@ static const struct pios_spi_cfg pios_spi_op_mag_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), @@ -175,7 +174,6 @@ static const struct pios_spi_cfg pios_spi_accel_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .init = { .NVIC_IRQChannel = DMA1_Channel2_IRQn, @@ -277,7 +275,6 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -321,7 +318,6 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART4_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -350,6 +346,21 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { #endif /* PIOS_COM_AUX */ +#if defined(PIOS_INCLUDE_COM) + +#include + +#if 0 +#define PIOS_COM_AUX_TX_BUF_LEN 192 +static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; +#endif + +#define PIOS_COM_GPS_RX_BUF_LEN 96 +static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN]; + + +#endif /* PIOS_INCLUDE_COM */ + #if defined(PIOS_INCLUDE_I2C) #include @@ -393,7 +404,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = { }, }, .event = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C1_EV_IRQn, @@ -403,7 +413,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = { }, }, .error = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C1_ER_IRQn, @@ -461,7 +470,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = { }, }, .event = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_EV_IRQn, @@ -471,7 +479,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = { }, }, .error = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_ER_IRQn, @@ -525,7 +532,9 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_DEBUG_Assert(0); } - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer), + NULL, 0)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 87daf3c52..682484543 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -329,6 +329,13 @@ static void updateAttitude(AttitudeRawData * attitudeRaw) q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; + + if(q[0] < 0) { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } } // Renomalize diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 163c3f577..dd0905257 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -196,8 +196,10 @@ static void gpsTask(void *parameters) while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) { - int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors); - if (res >= 0) + uint8_t c; + PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0); + + if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0) { numUpdates++; @@ -213,7 +215,8 @@ static void gpsTask(void *parameters) // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) { - char c = PIOS_COM_ReceiveBuffer(gpsPort); + uint8_t c; + PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0); // detect start while acquiring stream if (!start_flag && (c == '$')) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 665c0188c..9d3a538d0 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -84,6 +84,7 @@ uint8_t max_axis_lock = 0; uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; +bool lowThrottleZeroIntegral; pid_type pids[PID_MAX]; @@ -329,6 +330,7 @@ static void stabilizationTask(void* parameters) } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || + (lowThrottleZeroIntegral && stabDesired.Throttle < 0) || !shouldUpdate) { ZeroPids(); @@ -427,6 +429,9 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) weak_leveling_kp = settings.WeakLevelingKp; weak_leveling_max = settings.MaxWeakLevelingRate; + // Whether to zero the PID integrals while throttle is low + lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant // based on a time (in ms) rather than a fixed multiplier. The error between diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index ad0e39278..06581fe1a 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -305,7 +305,6 @@ static void telemetryTxPriTask(void *parameters) static void telemetryRxTask(void *parameters) { uint32_t inputPort; - int32_t len; // Task loop while (1) { @@ -319,14 +318,20 @@ static void telemetryRxTask(void *parameters) inputPort = telemetryPort; } - // Block until data are available - // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking - len = PIOS_COM_ReceiveBufferUsed(inputPort); - for (int32_t n = 0; n < len; ++n) { - UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort)); - } - vTaskDelay(5); // <- remove when blocking calls are implemented + if (inputPort) { + // Block until data are available + uint8_t serial_data[1]; + uint16_t bytes_to_process; + bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) { + UAVTalkProcessInputStream(serial_data[i]); + } + } + } else { + vTaskDelay(5); + } } } @@ -350,7 +355,11 @@ static int32_t transmitData(uint8_t * data, int32_t length) outputPort = telemetryPort; } - return PIOS_COM_SendBufferNonBlocking(outputPort, data, length); + if (outputPort) { + return PIOS_COM_SendBufferNonBlocking(outputPort, data, length); + } else { + return -1; + } } /** @@ -506,20 +515,16 @@ static void updateSettings() // Retrieve settings TelemetrySettingsGet(&settings); - // Set port speed - if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600); - else - if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200); + if (telemetryPort) { + // Set port speed + if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600); + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200); + } } /** diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index e349f368f..f93e93644 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -173,6 +173,8 @@ SRC += $(PIOSPOSIX)/pios_servo.c SRC += $(PIOSPOSIX)/pios_wdg.c SRC += $(PIOSPOSIX)/pios_debug.c +SRC += $(PIOSPOSIX)/pios_rcvr.c + ## Libraries for flight calculations #SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c diff --git a/flight/OpenPilot/System/inc/pios_config_posix.h b/flight/OpenPilot/System/inc/pios_config_posix.h index 136e264c3..99505c55d 100644 --- a/flight/OpenPilot/System/inc/pios_config_posix.h +++ b/flight/OpenPilot/System/inc/pios_config_posix.h @@ -38,7 +38,10 @@ #define PIOS_INCLUDE_COM #define PIOS_INCLUDE_UDP #define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_RCVR +#define PIOS_RCVR_MAX_CHANNELS 12 +#define PIOS_RCVR_MAX_DEVS 3 /* Defaults for Logging */ #define LOG_FILENAME "PIOS.LOG" diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index ab3a99533..822b7a8ae 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -64,7 +64,6 @@ static const struct pios_spi_cfg pios_spi_sdcard_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .init = { .NVIC_IRQChannel = DMA1_Channel2_IRQn, @@ -163,7 +162,6 @@ static const struct pios_spi_cfg pios_spi_ahrs_cfg = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), .init = { .NVIC_IRQChannel = DMA1_Channel4_IRQn, @@ -263,7 +261,6 @@ static const struct pios_adc_cfg pios_adc_cfg = { .dma = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .init = { .NVIC_IRQChannel = DMA1_Channel1_IRQn, @@ -322,7 +319,6 @@ static const struct pios_usart_cfg pios_usart_telem_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART2_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -363,7 +359,6 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART3_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -404,7 +399,6 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -444,7 +438,6 @@ static const struct pios_rtc_cfg pios_rtc_main_cfg = { .clksrc = RCC_RTCCLKSource_HSE_Div128, .prescaler = 100, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = RTC_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, @@ -461,7 +454,7 @@ void PIOS_RTC_IRQ_Handler (void) #endif -#ifdef PIOS_COM_SPEKTRUM +#if defined(PIOS_INCLUDE_SPEKTRUM) /* * SPEKTRUM USART */ @@ -478,7 +471,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = { .USART_Mode = USART_Mode_Rx, }, .irq = { - .handler = PIOS_SPEKTRUM_irq_handler, .init = { .NVIC_IRQChannel = USART1_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, @@ -505,12 +497,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = { }; #include -static uint32_t pios_usart_spektrum_id; -void PIOS_USART_spektrum_irq_handler(void) -{ - PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); -} - static const struct pios_spektrum_cfg pios_spektrum_cfg = { .bind = { .gpio = GPIOA, @@ -535,6 +521,14 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = { #include "pios_com_priv.h" +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 + +#define PIOS_COM_GPS_RX_BUF_LEN 96 + +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + #endif /* PIOS_INCLUDE_COM */ /** @@ -710,7 +704,6 @@ const struct pios_pwm_cfg pios_pwm_cfg = { }, .remap = GPIO_PartialRemap_TIM3, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, @@ -750,7 +743,6 @@ static const struct pios_ppmsv_cfg pios_ppmsv_cfg = { .TIM_RepetitionCounter = 0x0000, }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, @@ -790,7 +782,6 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { }, .remap = 0, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, @@ -851,7 +842,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .event = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_EV_IRQn, @@ -861,7 +851,6 @@ static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { }, }, .error = { - .handler = NULL, .flags = 0, /* FIXME: check this */ .init = { .NVIC_IRQChannel = I2C2_ER_IRQn, @@ -979,6 +968,21 @@ struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS] uint32_t pios_rcvr_max_channel; #endif /* PIOS_INCLUDE_RCVR */ +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_rf_id; @@ -1046,22 +1050,37 @@ void PIOS_Board_Init(void) { /* Initialize the PiOS library */ #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) - uint32_t pios_usart_telem_rf_id; - if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { - PIOS_Assert(0); - } - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { - PIOS_Assert(0); + { + uint32_t pios_usart_telem_rf_id; + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { + PIOS_Assert(0); + } + + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_telem_rf_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 */ #if defined(PIOS_INCLUDE_GPS) - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { - PIOS_Assert(0); + { + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } } #endif /* PIOS_INCLUDE_GPS */ #endif @@ -1118,23 +1137,28 @@ void PIOS_Board_Init(void) { #if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) #error More receiver inputs than available devices #endif - /* SPEKTRUM init must come before comms */ - PIOS_SPEKTRUM_Init(&pios_spektrum_cfg, false); + { + uint32_t pios_usart_spektrum_id; + if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { + PIOS_Assert(0); + } - uint32_t pios_usart_spektrum_id; - if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { - PIOS_Assert(0); - } - uint32_t pios_spektrum_rcvr_id; - if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; + uint32_t pios_spektrum_id; + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + PIOS_Assert(0); + } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { + PIOS_Assert(0); + } + for (uint8_t i = 0; + i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); + i++) { + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; + pios_rcvr_max_channel++; + } } #endif break; @@ -1145,15 +1169,21 @@ void PIOS_Board_Init(void) { break; } - #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_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 */ -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 87d8e7402..07cb6868d 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -29,6 +29,17 @@ #include #include +#include "pios_rcvr_priv.h" + +struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; +uint32_t pios_rcvr_max_channel; + +void Stack_Change() { +} + +void Stack_Change_Weak() { +} + /** * PIOS_Board_Init() * initializes all the core systems on this specific hardware diff --git a/flight/PiOS.posix/inc/pios_initcall.h b/flight/PiOS.posix/inc/pios_initcall.h index 32b3f7475..deb6e0eb6 100644 --- a/flight/PiOS.posix/inc/pios_initcall.h +++ b/flight/PiOS.posix/inc/pios_initcall.h @@ -38,11 +38,21 @@ * and we cannot define a linker script for each of them atm */ + +typedef int32_t (*initcall_t)(void); +typedef struct { + initcall_t fn_minit; + initcall_t fn_tinit; +} initmodule_t; + +/* Init module section */ +extern initmodule_t __module_initcall_start[], __module_initcall_end[]; + extern void InitModules(); extern void StartModules(); #define UAVOBJ_INITCALL(fn) -#define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags) +#define MODULE_INITCALL(ifn, sfn) #define MODULE_TASKCREATE_ALL { \ /* Start all module threads */ \ diff --git a/flight/PiOS.posix/inc/pios_rcvr.h b/flight/PiOS.posix/inc/pios_rcvr.h new file mode 100644 index 000000000..a32160894 --- /dev/null +++ b/flight/PiOS.posix/inc/pios_rcvr.h @@ -0,0 +1,54 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RCVR RCVR layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_rcvr.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief RCVR layer functions 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 PIOS_RCVR_H +#define PIOS_RCVR_H + +struct pios_rcvr_channel_map { + uint32_t id; + uint8_t channel; +}; + +extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; + +struct pios_rcvr_driver { + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); +}; + +/* Public Functions */ +extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); + +#endif /* PIOS_RCVR_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/inc/pios_rcvr_priv.h b/flight/PiOS.posix/inc/pios_rcvr_priv.h new file mode 100644 index 000000000..968dc2116 --- /dev/null +++ b/flight/PiOS.posix/inc/pios_rcvr_priv.h @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RCVR RCVR Functions + * @brief PIOS interface for RCVR drivers + * @{ + * + * @file pios_rcvr_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) + * @brief USART private 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_RCVR_PRIV_H +#define PIOS_RCVR_PRIV_H + +#include + +extern uint32_t pios_rcvr_max_channel; + +extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id); + +extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id); + +#endif /* PIOS_RCVR_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/pios.h b/flight/PiOS.posix/pios.h index a82d13b81..12c26c64d 100644 --- a/flight/PiOS.posix/pios.h +++ b/flight/PiOS.posix/pios.h @@ -65,6 +65,7 @@ #include #include #include +#include #define NELEMENTS(x) (sizeof(x) / sizeof(*(x))) diff --git a/flight/PiOS.posix/posix/pios_delay.c b/flight/PiOS.posix/posix/pios_delay.c index c295b4ac9..cec10467c 100644 --- a/flight/PiOS.posix/posix/pios_delay.c +++ b/flight/PiOS.posix/posix/pios_delay.c @@ -67,10 +67,6 @@ int32_t PIOS_DELAY_WaituS(uint16_t uS) while (!nanosleep(&wait,&rest)) { wait=rest; } - //uint16_t start = PIOS_DELAY_TIMER->CNT; - - /* Note that this event works on 16bit counter wrap-arounds */ - //while((uint16_t)(PIOS_DELAY_TIMER->CNT - start) <= uS); /* No error */ return 0; diff --git a/flight/PiOS.posix/posix/pios_rcvr.c b/flight/PiOS.posix/posix/pios_rcvr.c new file mode 100644 index 000000000..652730547 --- /dev/null +++ b/flight/PiOS.posix/posix/pios_rcvr.c @@ -0,0 +1,100 @@ +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_RCVR) + +#include + +enum pios_rcvr_dev_magic { + PIOS_RCVR_DEV_MAGIC = 0x99aabbcc, +}; + +struct pios_rcvr_dev { + enum pios_rcvr_dev_magic magic; + uint32_t lower_id; + const struct pios_rcvr_driver * driver; +}; + +static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) +{ + return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +{ + struct pios_rcvr_dev * rcvr_dev; + + rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); + if (!rcvr_dev) return (NULL); + + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + return(rcvr_dev); +} +#else +static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS]; +static uint8_t pios_rcvr_num_devs; +static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) +{ + struct pios_rcvr_dev * rcvr_dev; + + if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) { + return (NULL); + } + + rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++]; + rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; + + return (rcvr_dev); +} +#endif + +/** + * Initialises RCVR layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id) +{ + PIOS_DEBUG_Assert(rcvr_id); + PIOS_DEBUG_Assert(driver); + + struct pios_rcvr_dev * rcvr_dev; + + rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc(); + if (!rcvr_dev) goto out_fail; + + rcvr_dev->driver = driver; + rcvr_dev->lower_id = lower_id; + + + *rcvr_id = pios_rcvr_num_devs - 1; + + return(0); + +out_fail: + return(-1); +} + +int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id]; + + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_DEBUG_Assert(0); + } + + PIOS_DEBUG_Assert(rcvr_dev->driver->read); + + return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/PiOS.win32/win32/pios_delay.c b/flight/PiOS.win32/win32/pios_delay.c index 0fed0d8d4..a8d892601 100644 --- a/flight/PiOS.win32/win32/pios_delay.c +++ b/flight/PiOS.win32/win32/pios_delay.c @@ -62,11 +62,7 @@ int32_t PIOS_DELAY_Init(void) int32_t PIOS_DELAY_WaituS(uint16_t uS) { Sleep(uS/1000); - //uint16_t start = PIOS_DELAY_TIMER->CNT; - /* Note that this event works on 16bit counter wrap-arounds */ - //while((uint16_t)(PIOS_DELAY_TIMER->CNT - start) <= uS); - /* No error */ return 0; } diff --git a/flight/PiOS/Boards/STM32103CB_AHRS.h b/flight/PiOS/Boards/STM32103CB_AHRS.h index 611d285fc..26d88fce1 100644 --- a/flight/PiOS/Boards/STM32103CB_AHRS.h +++ b/flight/PiOS/Boards/STM32103CB_AHRS.h @@ -79,12 +79,6 @@ TIM8 | | | | #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN } #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK } -//------------------------- -// Delay Timer -//------------------------- -#define PIOS_DELAY_TIMER TIM2 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) - //------------------------- // System Settings //------------------------- @@ -118,8 +112,6 @@ extern uint32_t pios_i2c_main_adapter_id; // PIOS_USART //------------------------- #define PIOS_USART_MAX_DEVS 2 -#define PIOS_USART_RX_BUFFER_SIZE 256 -#define PIOS_USART_TX_BUFFER_SIZE 256 //------------------------- // PIOS_COM @@ -127,6 +119,7 @@ extern uint32_t pios_i2c_main_adapter_id; // See also pios_board.c //------------------------- #define PIOS_COM_MAX_DEVS 2 + extern uint32_t pios_com_aux_id; #define PIOS_COM_AUX (pios_com_aux_id) #define PIOS_COM_DEBUG PIOS_COM_AUX diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 38a29868c..18e0da624 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -91,12 +91,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN } #define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK } -//------------------------- -// Delay Timer -//------------------------- -#define PIOS_DELAY_TIMER TIM3 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE) - //------------------------- // System Settings //------------------------- @@ -131,15 +125,12 @@ extern uint32_t pios_i2c_main_adapter_id; //------------------------- #define PIOS_USART_MAX_DEVS 2 -#define PIOS_USART_RX_BUFFER_SIZE 256 -#define PIOS_USART_TX_BUFFER_SIZE 256 - //------------------------- // PIOS_COM // // See also pios_board.c //------------------------- -#define PIOS_COM_MAX_DEVS 4 +#define PIOS_COM_MAX_DEVS 3 extern uint32_t pios_com_telem_rf_id; #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) @@ -153,16 +144,6 @@ extern uint32_t pios_com_gps_id; extern uint32_t pios_com_telem_usb_id; #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#ifdef PIOS_INCLUDE_SPEKTRUM -extern uint32_t pios_com_spektrum_id; -#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) -#endif - -#ifdef PIOS_INCLUDE_SBUS -extern uint32_t pios_com_sbus_id; -#define PIOS_COM_SBUS (pios_com_sbus_id) -#endif - //------------------------- // ADC // PIOS_ADC_PinGet(0) = Gyro Z @@ -231,7 +212,7 @@ extern uint32_t pios_com_sbus_id; //------------------------- // Receiver PPM input //------------------------- -#define PIOS_PPM_NUM_INPUTS 6 //Could be more if needed +#define PIOS_PPM_NUM_INPUTS 12 //------------------------- // Receiver PWM input @@ -271,10 +252,9 @@ extern uint32_t pios_com_sbus_id; // USB //------------------------- #define PIOS_USB_ENABLED 1 +#define PIOS_USB_HID_MAX_DEVS 1 #define PIOS_USB_DETECT_GPIO_PORT GPIOC #define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 #define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 #define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID -#define PIOS_USB_RX_BUFFER_SIZE 128 -#define PIOS_USB_TX_BUFFER_SIZE 256 #endif /* STM32103CB_AHRS_H_ */ diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index a8eead4bd..7027a42bd 100644 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -117,14 +117,6 @@ TIM4 | STOPWATCH | #define TX_LED_OFF PIOS_LED_Off(LED4) #define TX_LED_TOGGLE PIOS_LED_Toggle(LED4) -// ***************************************************************** -// Delay Timer - -//#define PIOS_DELAY_TIMER TIM2 -//#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) -#define PIOS_DELAY_TIMER TIM1 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE) - // ***************************************************************** // Timer interrupt @@ -152,9 +144,6 @@ extern uint32_t pios_spi_port_id; //------------------------- #define PIOS_USART_MAX_DEVS 1 -#define PIOS_USART_RX_BUFFER_SIZE 512 -#define PIOS_USART_TX_BUFFER_SIZE 512 - //------------------------- // PIOS_COM // @@ -404,12 +393,11 @@ extern uint32_t pios_com_telem_usb_id; #if defined(PIOS_INCLUDE_USB_HID) #define PIOS_USB_ENABLED 1 + #define PIOS_USB_HID_MAX_DEVS 1 #define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT #define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN #define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 #define PIOS_IRQ_USB_PRIORITY 8 - #define PIOS_USB_RX_BUFFER_SIZE 512 - #define PIOS_USB_TX_BUFFER_SIZE 512 #endif // ***************************************************************** diff --git a/flight/PiOS/Boards/STM3210E_INS.h b/flight/PiOS/Boards/STM3210E_INS.h index 505db9bb6..d661e8b96 100644 --- a/flight/PiOS/Boards/STM3210E_INS.h +++ b/flight/PiOS/Boards/STM3210E_INS.h @@ -125,9 +125,6 @@ extern uint32_t pios_i2c_gyro_adapter_id; //------------------------- #define PIOS_USART_MAX_DEVS 2 -#define PIOS_USART_RX_BUFFER_SIZE 256 -#define PIOS_USART_TX_BUFFER_SIZE 256 - //------------------------- // PIOS_COM // @@ -144,12 +141,6 @@ extern uint32_t pios_com_aux_id; #define PIOS_COM_DEBUG PIOS_COM_AUX #endif -//------------------------- -// Delay Timer -//------------------------- -#define PIOS_DELAY_TIMER TIM2 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) - //------------------------- // System Settings //------------------------- diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index c25e6ccfa..3098ee259 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -139,9 +139,6 @@ extern uint32_t pios_i2c_main_adapter_id; //------------------------- #define PIOS_USART_MAX_DEVS 3 -#define PIOS_USART_RX_BUFFER_SIZE 512 -#define PIOS_USART_TX_BUFFER_SIZE 512 - //------------------------- // PIOS_COM // @@ -164,22 +161,6 @@ extern uint32_t pios_com_aux_id; #define PIOS_COM_DEBUG PIOS_COM_AUX #endif -#ifdef PIOS_INCLUDE_SPEKTRUM -extern uint32_t pios_com_spektrum_id; -#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id) -#endif - -#ifdef PIOS_INCLUDE_SBUS -extern uint32_t pios_com_sbus_id; -#define PIOS_COM_SBUS (pios_com_sbus_id) -#endif - -//------------------------- -// Delay Timer -//------------------------- -#define PIOS_DELAY_TIMER TIM2 -#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE) - //------------------------- // System Settings //------------------------- @@ -204,8 +185,7 @@ extern uint32_t pios_com_sbus_id; //------------------------- // Receiver PPM input //------------------------- -#define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed -#define PIOS_PPM_SUPV_ENABLED 1 +#define PIOS_PPM_NUM_INPUTS 12 //------------------------- // Receiver PWM input @@ -315,12 +295,11 @@ extern uint32_t pios_com_sbus_id; // USB //------------------------- #define PIOS_USB_ENABLED 1 +#define PIOS_USB_HID_MAX_DEVS 1 #define PIOS_USB_DETECT_GPIO_PORT GPIOC #define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_4 #define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 #define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID -#define PIOS_USB_RX_BUFFER_SIZE 512 -#define PIOS_USB_TX_BUFFER_SIZE 512 /** * glue macros for file IO diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c index 4bd00c22b..b7a9a67e5 100644 --- a/flight/PiOS/Common/pios_com.c +++ b/flight/PiOS/Common/pios_com.c @@ -34,8 +34,34 @@ #if defined(PIOS_INCLUDE_COM) +#include "fifo_buffer.h" #include +#if !defined(PIOS_INCLUDE_FREERTOS) +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#endif + +enum pios_com_dev_magic { + PIOS_COM_DEV_MAGIC = 0xaa55aa55, +}; + +struct pios_com_dev { + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver * driver; + +#if defined(PIOS_INCLUDE_FREERTOS) + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; +#endif + + bool has_rx; + bool has_tx; + + t_fifo_buffer rx; + t_fifo_buffer tx; +}; + static bool PIOS_COM_validate(struct pios_com_dev * com_dev) { return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); @@ -70,6 +96,11 @@ static struct pios_com_dev * PIOS_COM_alloc(void) } #endif +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); + /** * Initialises COM layer * \param[out] handle @@ -77,18 +108,48 @@ static struct pios_com_dev * PIOS_COM_alloc(void) * \param[in] id * \return < 0 if initialisation failed */ -int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, const uint32_t lower_id) +int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) { - PIOS_DEBUG_Assert(com_id); - PIOS_DEBUG_Assert(driver); + PIOS_Assert(com_id); + PIOS_Assert(driver); + + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); struct pios_com_dev * com_dev; com_dev = (struct pios_com_dev *) PIOS_COM_alloc(); if (!com_dev) goto out_fail; - com_dev->driver = driver; - com_dev->id = lower_id; + com_dev->driver = driver; + com_dev->lower_id = lower_id; + + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; + + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); +#if defined(PIOS_INCLUDE_FREERTOS) + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } + + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); +#if defined(PIOS_INCLUDE_FREERTOS) + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev); + } *com_id = (uint32_t)com_dev; return(0); @@ -97,6 +158,88 @@ out_fail: return(-1); } +static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); + + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } +#else + *need_yield = false; +#endif +} + +static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } +#else + *need_yield = false; +#endif +} + +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + + bool valid = PIOS_COM_validate(com_dev); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); + + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); + + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } + + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return (bytes_into_fifo); +} + +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_dev * com_dev = (struct pios_com_dev *)context; + + bool valid = PIOS_COM_validate(com_dev); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); + + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); + + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } + + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return (bytes_from_fifo); +} + /** * Change the port speed without re-initializing * \param[in] port COM port @@ -115,7 +258,7 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) /* Invoke the driver function if it exists */ if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->id, baud); + com_dev->driver->set_baud(com_dev->lower_id, baud); } return 0; @@ -140,12 +283,26 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u return -1; } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx_nb) { - return com_dev->driver->tx_nb(com_dev->id, buffer, len); + PIOS_Assert(com_dev->has_tx); + + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; } - return 0; + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); + + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } + + return (0); } /** @@ -166,12 +323,27 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len return -1; } - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx) { - return com_dev->driver->tx(com_dev->id, buffer, len); - } + PIOS_Assert(com_dev->has_tx); - return 0; + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + +#if defined(PIOS_INCLUDE_FREERTOS) + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } +#endif + } while (rc == -2); + + return rc; } /** @@ -273,18 +445,47 @@ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) * \param[in] port COM port * \returns Byte from buffer */ -uint8_t PIOS_COM_ReceiveBuffer(uint32_t com_id) +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) { + PIOS_Assert(buf); + PIOS_Assert(buf_len); + struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; if (!PIOS_COM_validate(com_dev)) { /* Undefined COM port for this board (see pios_board.c) */ - PIOS_DEBUG_Assert(0); + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); + + check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); + + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } +#if defined(PIOS_INCLUDE_FREERTOS) + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } +#else + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; +#endif } - PIOS_DEBUG_Assert(com_dev->driver->rx); - - return com_dev->driver->rx(com_dev->id); + /* Return received byte */ + return (bytes_from_fifo); } /** @@ -298,16 +499,11 @@ int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id) if (!PIOS_COM_validate(com_dev)) { /* Undefined COM port for this board (see pios_board.c) */ - /* This is commented out so com_id=NULL can be used to disable telemetry */ - //PIOS_DEBUG_Assert(0); - return 0; + PIOS_Assert(0); } - if (!com_dev->driver->rx_avail) { - return 0; - } - - return com_dev->driver->rx_avail(com_dev->id); + PIOS_Assert(com_dev->has_rx); + return (fifoBuf_getUsed(&com_dev->rx)); } #endif diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index f43fee69f..29acb2594 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -56,7 +56,7 @@ static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) * \param[in] id * \return < 0 if initialisation failed */ -int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id) +int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id) { PIOS_DEBUG_Assert(rcvr_id); PIOS_DEBUG_Assert(driver); @@ -82,7 +82,7 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) if (!PIOS_RCVR_validate(rcvr_dev)) { /* Undefined RCVR port for this board (see pios_board.c) */ - PIOS_DEBUG_Assert(0); + PIOS_Assert(0); } PIOS_DEBUG_Assert(rcvr_dev->driver->read); diff --git a/flight/PiOS/STM32F10x/pios_delay.c b/flight/PiOS/STM32F10x/pios_delay.c index 56c556555..c4e74717b 100644 --- a/flight/PiOS/STM32F10x/pios_delay.c +++ b/flight/PiOS/STM32F10x/pios_delay.c @@ -7,10 +7,10 @@ * @{ * * @file pios_delay.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @author Michael Smith Copyright (C) 2011 * @brief Delay Functions - * - Provides a micro-second granular delay using a TIM + * - Provides a micro-second granular delay using the CPU + * cycle counter. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ @@ -31,72 +31,97 @@ */ /* Project Includes */ -#include "pios.h" +#include #if defined(PIOS_INCLUDE_DELAY) +/* these should be defined by CMSIS, but they aren't */ +#define DWT_CTRL (*(volatile uint32_t *)0xe0001000) +#define CYCCNTENA (1<<0) +#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004) + + +/* cycles per microsecond */ +static uint32_t us_ticks; + /** -* Initialises the Timer used by PIOS_DELAY functions
-* This is called from pios.c as part of the main() function -* at system start up. -* \return < 0 if initialisation failed -*/ + * Initialises the Timer used by PIOS_DELAY functions. + * + * \return always zero (success) + */ int32_t PIOS_DELAY_Init(void) { - /* Enable timer clock */ - PIOS_DELAY_TIMER_RCC_FUNC; + RCC_ClocksTypeDef clocks; - /* Time base configuration */ - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = 65535; // maximum value - TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1; // for 1 uS accuracy fixed to 72Mhz - TIM_TimeBaseStructure.TIM_ClockDivision = 0; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(PIOS_DELAY_TIMER, &TIM_TimeBaseStructure); + /* compute the number of system clocks per microsecond */ + RCC_GetClocksFreq(&clocks); + us_ticks = clocks.SYSCLK_Frequency / 1000000; + PIOS_DEBUG_Assert(us_ticks > 1); - /* Enable counter */ - TIM_Cmd(PIOS_DELAY_TIMER, ENABLE); + /* turn on access to the DWT registers */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + /* enable the CPU cycle counter */ + DWT_CTRL |= CYCCNTENA; + + return 0; +} + +/** + * Waits for a specific number of uS + * + * Example:
+ * \code + * // Wait for 500 uS + * PIOS_DELAY_Wait_uS(500); + * \endcode + * \param[in] uS delay + * \return < 0 on errors + */ +int32_t PIOS_DELAY_WaituS(uint32_t uS) +{ + uint32_t elapsed = 0; + uint32_t last_count = DWT_CYCCNT; + + for (;;) { + uint32_t current_count = DWT_CYCCNT; + uint32_t elapsed_uS; + + /* measure the time elapsed since the last time we checked */ + elapsed += current_count - last_count; + last_count = current_count; + + /* convert to microseconds */ + elapsed_uS = elapsed / us_ticks; + if (elapsed_uS >= uS) + break; + + /* reduce the delay by the elapsed time */ + uS -= elapsed_uS; + + /* keep fractional microseconds for the next iteration */ + elapsed %= us_ticks; + } /* No error */ return 0; } /** -* Waits for a specific number of uS
-* Example:
-* \code -* // Wait for 500 uS -* PIOS_DELAY_Wait_uS(500); -* \endcode -* \param[in] uS delay (1..65535 microseconds) -* \return < 0 on errors -*/ -int32_t PIOS_DELAY_WaituS(uint16_t uS) + * Waits for a specific number of mS + * + * Example:
+ * \code + * // Wait for 500 mS + * PIOS_DELAY_Wait_mS(500); + * \endcode + * \param[in] mS delay + * \return < 0 on errors + */ +int32_t PIOS_DELAY_WaitmS(uint32_t mS) { - uint16_t start = PIOS_DELAY_TIMER->CNT; - - /* Note that this event works on 16bit counter wrap-arounds */ - while ((uint16_t) (PIOS_DELAY_TIMER->CNT - start) <= uS) ; - - /* No error */ - return 0; -} - -/** -* Waits for a specific number of mS
-* Example:
-* \code -* // Wait for 500 mS -* PIOS_DELAY_Wait_mS(500); -* \endcode -* \param[in] mS delay (1..65535 milliseconds) -* \return < 0 on errors -*/ -int32_t PIOS_DELAY_WaitmS(uint16_t mS) -{ - for (int i = 0; i < mS; i++) { + while (mS--) { PIOS_DELAY_WaituS(1000); } @@ -108,22 +133,19 @@ int32_t PIOS_DELAY_WaitmS(uint16_t mS) * @brief Query the Delay timer for the current uS * @return A microsecond value */ -uint16_t PIOS_DELAY_GetuS() +uint32_t PIOS_DELAY_GetuS(void) { - return PIOS_DELAY_TIMER->CNT; + return DWT_CYCCNT / us_ticks; } /** - * @brief Compute the difference between now and a reference time - * @param[in] the reference time to compare now to - * @return The number of uS since the delay - * - * @note the user is responsible for worrying about rollover on the 16 bit uS counter + * @brief Calculate time in microseconds since a previous time + * @param[in] t previous time + * @return time in us since previous time t. */ -int32_t PIOS_DELAY_DiffuS(uint16_t ref) +uint32_t PIOS_DELAY_GetuSSince(uint32_t t) { - int32_t ret_t = ref; - return (int16_t) (PIOS_DELAY_GetuS() - ret_t); + return (PIOS_DELAY_GetuS() - t); } #endif diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index 851fe217a..fe7421a17 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -41,19 +41,30 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { .read = PIOS_PPM_Get, }; +#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 +#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS +#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames +#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds +#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PIOS_PPM_INPUT_INVALID 0 + /* Local Variables */ static TIM_ICInitTypeDef TIM_ICInitStructure; static uint8_t PulseIndex; -static uint32_t PreviousValue; -static uint32_t CurrentValue; -static uint32_t CapturedValue; -static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS]; -static uint32_t CapCounter[PIOS_PPM_NUM_INPUTS]; -static uint16_t TimerCounter; +static uint32_t PreviousTime; +static uint32_t CurrentTime; +static uint32_t DeltaTime; +static uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; +static uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; +static uint32_t LargeCounter; +static int8_t NumChannels; +static int8_t NumChannelsPrevFrame; +static uint8_t NumChannelCounter; static uint8_t supv_timer = 0; -static uint8_t SupervisorState = 0; -static uint32_t CapCounterPrev[PIOS_PPM_NUM_INPUTS]; +static bool Tracking; +static bool Fresh; static void PIOS_PPM_Supervisor(uint32_t ppm_id); @@ -63,13 +74,19 @@ void PIOS_PPM_Init(void) int32_t i; PulseIndex = 0; - PreviousValue = 0; - CurrentValue = 0; - CapturedValue = 0; - TimerCounter = 0; + PreviousTime = 0; + CurrentTime = 0; + DeltaTime = 0; + LargeCounter = 0; + NumChannels = -1; + NumChannelsPrevFrame = -1; + NumChannelCounter = 0; + Tracking = FALSE; + Fresh = FALSE; - for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { + for (i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { CaptureValue[i] = 0; + CaptureValueNewFrame[i] = 0; } NVIC_InitTypeDef NVIC_InitStructure = pios_ppm_cfg.irq.init; @@ -93,7 +110,6 @@ void PIOS_PPM_Init(void) RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); break; #ifdef STM32F10X_HD - case (int32_t)TIM5: NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); @@ -134,15 +150,6 @@ void PIOS_PPM_Init(void) /* Enable timers */ TIM_Cmd(pios_ppm_cfg.timer, ENABLE); - /* Supervisor Setup */ - /* Flush counter variables */ - for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { - CapCounter[i] = 0; - } - for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { - CapCounterPrev[i] = 0; - } - /* Setup local variable which stays in this scope */ /* Doing this here and using a local variable saves doing it in the ISR */ TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; @@ -163,7 +170,7 @@ void PIOS_PPM_Init(void) static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { /* Return error if channel not available */ - if (channel >= PIOS_PPM_NUM_INPUTS) { + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { return -1; } return CaptureValue[channel]; @@ -176,56 +183,103 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) */ void PIOS_PPM_irq_handler(void) { + /* Timer Overflow Interrupt + * The time between timer overflows must be greater than the PPM + * frame period. If a full frame has not decoded in the between timer + * overflows then capture values should be cleared. + */ + if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) { - TimerCounter+=pios_ppm_cfg.timer->ARR; + /* Clear TIMx overflow interrupt pending bit */ TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update); - if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) != SET) { - return; - } + + /* If sharing a timer with a servo output the ARR register will + be set according to the PWM period. When timer reaches the + ARR value a timer overflow interrupt will fire. We use the + interrupt accumulate a 32-bit timer. */ + LargeCounter = LargeCounter + pios_ppm_cfg.timer->ARR; } - - /* Do this as it's more efficient */ + /* Signal edge interrupt */ if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) { - PreviousValue = CurrentValue; + PreviousTime = CurrentTime; + switch((int32_t) pios_ppm_cfg.ccr) { case (int32_t)TIM_IT_CC1: - CurrentValue = TIM_GetCapture1(pios_ppm_cfg.timer); + CurrentTime = TIM_GetCapture1(pios_ppm_cfg.timer); break; case (int32_t)TIM_IT_CC2: - CurrentValue = TIM_GetCapture2(pios_ppm_cfg.timer); + CurrentTime = TIM_GetCapture2(pios_ppm_cfg.timer); break; case (int32_t)TIM_IT_CC3: - CurrentValue = TIM_GetCapture3(pios_ppm_cfg.timer); + CurrentTime = TIM_GetCapture3(pios_ppm_cfg.timer); break; case (int32_t)TIM_IT_CC4: - CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer); + CurrentTime = TIM_GetCapture4(pios_ppm_cfg.timer); break; } - CurrentValue+=TimerCounter; - if(CurrentValue > 0xFFFF) { - CurrentValue-=0xFFFF; - } /* Clear TIMx Capture compare interrupt pending bit */ TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr); - /* Capture computation */ - if (CurrentValue > PreviousValue) { - CapturedValue = (CurrentValue - PreviousValue); - } else { - CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue); - } + /* Convert to 32-bit timer result */ + CurrentTime = CurrentTime + LargeCounter; - /* sync pulse */ - if (CapturedValue > 8000) { + /* Capture computation */ + DeltaTime = CurrentTime - PreviousTime; + + PreviousTime = CurrentTime; + + /* Sync pulse detection */ + if (DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (PulseIndex == NumChannelsPrevFrame + && PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) + { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) + NumChannelCounter++; + else + NumChannels = PulseIndex; + } else { + NumChannelCounter = 0; + } + + /* Check if the last frame was well formed */ + if (PulseIndex == NumChannels && Tracking) { + /* The last frame was well formed */ + for (uint32_t i = 0; i < NumChannels; i++) { + CaptureValue[i] = CaptureValueNewFrame[i]; + } + for (uint32_t i = NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + CaptureValue[i] = PIOS_PPM_INPUT_INVALID; + } + } + + Fresh = TRUE; + Tracking = TRUE; + NumChannelsPrevFrame = PulseIndex; PulseIndex = 0; - /* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */ - } else if (CapturedValue > 750 && CapturedValue < 2500) { - if (PulseIndex < PIOS_PPM_NUM_INPUTS) { - CaptureValue[PulseIndex] = CapturedValue; - CapCounter[PulseIndex]++; + + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ + + } else if (Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + + CaptureValueNewFrame[PulseIndex] = DeltaTime; PulseIndex++; + } else { + /* Not a valid pulse duration */ + Tracking = FALSE; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { + CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + } } } } @@ -241,26 +295,16 @@ static void PIOS_PPM_Supervisor(uint32_t ppm_id) { } supv_timer = 0; - /* Simple state machine */ - if (SupervisorState == 0) { - /* Save this states values */ - for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { - CapCounterPrev[i] = CapCounter[i]; - } + if (!Fresh) { + Tracking = FALSE; - /* Move to next state */ - SupervisorState = 1; - } else { - /* See what channels have been updated */ - for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { - if (CapCounter[i] == CapCounterPrev[i]) { - CaptureValue[i] = 0; - } + for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { + CaptureValue[i] = PIOS_PPM_INPUT_INVALID; + CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; } - - /* Move to next state */ - SupervisorState = 0; } + + Fresh = FALSE; } #endif diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index bb539cf87..433c272bb 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -35,7 +35,7 @@ #if defined(PIOS_INCLUDE_PWM) /* Provide a RCVR driver */ -static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t chan_id); +static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_pwm_rcvr_driver = { .read = PIOS_PWM_Get, diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 9f8be3e4a..a80fe8fe3 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -132,10 +132,30 @@ static void process_byte(uint8_t b) } } +static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + process_byte(buf[i]); + receive_timer = 0; + } + + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = SBUS_FRAME_LENGTH; + } + + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return (buf_len); +} + /** * Initialise S.Bus receiver interface */ -void PIOS_SBUS_Init(const struct pios_sbus_cfg *cfg) +int32_t PIOS_SBUS_Init(uint32_t * sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id) { /* Enable inverter clock and enable the inverter */ (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE); @@ -144,9 +164,13 @@ void PIOS_SBUS_Init(const struct pios_sbus_cfg *cfg) cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable); + (driver->bind_rx_cb)(lower_id, PIOS_SBUS_RxInCallback, 0); + if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, 0)) { PIOS_Assert(0); } + + return (0); } /** @@ -164,34 +188,6 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) return channel_data[channel]; } -/** - * Interrupt handler for USART - */ -void PIOS_SBUS_irq_handler(uint32_t usart_id) -{ - /* Grab the config for this device from the underlying USART device */ - const struct pios_usart_cfg * cfg; - cfg = PIOS_USART_GetConfig(usart_id); - PIOS_Assert(cfg); - - /* by always reading DR after SR make sure to clear any error interrupts */ - volatile uint16_t sr = cfg->regs->SR; - volatile uint8_t b = cfg->regs->DR; - - /* process received byte if one has arrived */ - if (sr & USART_SR_RXNE) { - /* process byte and clear receive timer */ - process_byte(b); - receive_timer = 0; - } - - /* ignore TXE interrupts */ - if (sr & USART_SR_TXE) { - /* disable TXE interrupt (TXEIE=0) */ - USART_ITConfig(cfg->regs, USART_IT_TXE, DISABLE); - } -} - /** * Input data supervisor is called periodically and provides * two functions: frame syncing and failsafe triggering. diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index ce6a5a4f5..8b0f38c6b 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -60,20 +60,45 @@ uint16_t supv_timer=0; static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id); static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg); +static int32_t PIOS_SPEKTRUM_Decode(uint8_t b); + +static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_SPEKTRUM_Decode(buf[i]); + supv_timer = 0; + } + + /* Always signal that we can accept another byte */ + if (headroom) { + *headroom = 1; + } + + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return (buf_len); +} /** * Bind and Initialise Spektrum satellite receiver */ -void PIOS_SPEKTRUM_Init(const struct pios_spektrum_cfg * cfg, bool bind) +int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind) { // TODO: need setting flag for bind on next powerup if (bind) { PIOS_SPEKTRUM_Bind(cfg); } + (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0); + if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) { PIOS_DEBUG_Assert(0); } + + return (0); } /** @@ -204,32 +229,6 @@ static int32_t PIOS_SPEKTRUM_Decode(uint8_t b) return 0; } -/* Custom interrupt handler for USART */ -void PIOS_SPEKTRUM_irq_handler(uint32_t usart_id) { - /* Grab the config for this device from the underlying USART device */ - const struct pios_usart_cfg * cfg; - cfg = PIOS_USART_GetConfig(usart_id); - PIOS_Assert(cfg); - - /* by always reading DR after SR make sure to clear any error interrupts */ - volatile uint16_t sr = cfg->regs->SR; - volatile uint8_t b = cfg->regs->DR; - - /* check if RXNE flag is set */ - if (sr & USART_SR_RXNE) { - if (PIOS_SPEKTRUM_Decode(b) < 0) { - /* Here we could add some error handling */ - } - } - - if (sr & USART_SR_TXE) { // check if TXE flag is set - /* Disable TXE interrupt (TXEIE=0) */ - USART_ITConfig(cfg->regs, USART_IT_TXE, DISABLE); - } - /* byte arrived so clear "watchdog" timer */ - supv_timer=0; -} - /** *@brief This function is called between frames and when a spektrum word hasnt been decoded for too long *@brief clears the channel values diff --git a/flight/PiOS/STM32F10x/pios_usart.c b/flight/PiOS/STM32F10x/pios_usart.c index 176992cb9..8582344b5 100644 --- a/flight/PiOS/STM32F10x/pios_usart.c +++ b/flight/PiOS/STM32F10x/pios_usart.c @@ -38,17 +38,17 @@ /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); -static int32_t PIOS_USART_TxBufferPutMoreNonBlocking(uint32_t usart_id, const uint8_t *buffer, uint16_t len); -static int32_t PIOS_USART_TxBufferPutMore(uint32_t usart_id, const uint8_t *buffer, uint16_t len); -static int32_t PIOS_USART_RxBufferGet(uint32_t usart_id); -static int32_t PIOS_USART_RxBufferUsed(uint32_t usart_id); +static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); +static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .tx_nb = PIOS_USART_TxBufferPutMoreNonBlocking, - .tx = PIOS_USART_TxBufferPutMore, - .rx = PIOS_USART_RxBufferGet, - .rx_avail = PIOS_USART_RxBufferUsed, + .set_baud = PIOS_USART_ChangeBaud, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { @@ -59,13 +59,10 @@ struct pios_usart_dev { enum pios_usart_dev_magic magic; const struct pios_usart_cfg * cfg; - // align to 32-bit to try and provide speed improvement; - uint8_t rx_buffer[PIOS_USART_RX_BUFFER_SIZE] __attribute__ ((aligned(4))); - t_fifo_buffer rx; - - // align to 32-bit to try and provide speed improvement; - uint8_t tx_buffer[PIOS_USART_TX_BUFFER_SIZE] __attribute__ ((aligned(4))); - t_fifo_buffer tx; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; }; static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) @@ -147,10 +144,6 @@ int32_t PIOS_USART_Init(uint32_t * usart_id, const struct pios_usart_cfg * cfg) /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; - /* Clear buffer counters */ - fifoBuf_init(&usart_dev->rx, usart_dev->rx_buffer, sizeof(usart_dev->rx_buffer)); - fifoBuf_init(&usart_dev->tx, usart_dev->tx_buffer, sizeof(usart_dev->tx_buffer)); - /* Enable the USART Pins Software Remapping */ if (usart_dev->cfg->remap) { GPIO_PinRemapConfig(usart_dev->cfg->remap, ENABLE); @@ -203,17 +196,23 @@ out_fail: return(-1); } -const struct pios_usart_cfg * PIOS_USART_GetConfig(uint32_t usart_id) +static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail) { struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; bool valid = PIOS_USART_validate(usart_dev); + PIOS_Assert(valid); - if (!valid) { - return (NULL); - } + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); +} +static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail) +{ + struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - return usart_dev->cfg; + bool valid = PIOS_USART_validate(usart_dev); + PIOS_Assert(valid); + + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); } /** @@ -240,170 +239,34 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) USART_Init(usart_dev->cfg->regs, &USART_InitStructure); } -/** -* Returns number of used bytes in receive buffer -* \param[in] USART USART name -* \return > 0: number of used bytes -* \return 0 if USART not available -*/ -static int32_t PIOS_USART_RxBufferUsed(uint32_t usart_id) +static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context) { struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) + PIOS_Assert(valid); - return (fifoBuf_getUsed(&usart_dev->rx)); + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->rx_in_context = context; + usart_dev->rx_in_cb = rx_in_cb; } -/** -* Gets a byte from the receive buffer -* \param[in] USART USART name -* \return -1 if no new byte available -* \return >= 0: actual byte received -*/ -static int32_t PIOS_USART_RxBufferGet(uint32_t usart_id) +static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context) { struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) + PIOS_Assert(valid); - if (fifoBuf_getUsed(&usart_dev->rx) == 0) { - /* Nothing new in the buffer */ - return -1; - } - - /* get byte - this operation should be atomic! */ - uint8_t b = fifoBuf_getByte(&usart_dev->rx); - - /* Return received byte */ - return b; -} - -/** -* puts a byte onto the receive buffer -* \param[in] USART USART name -* \param[in] b byte which should be put into Rx buffer -* \return 0 if no error -* \return -1 if buffer full (retry) -*/ -static int32_t PIOS_USART_RxBufferPut(uint32_t usart_id, uint8_t b) -{ - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) - - if (fifoBuf_getFree(&usart_dev->rx) < 1) { - /* Buffer full (retry) */ - return -1; - } - - /* Copy received byte into receive buffer */ - /* This operation should be atomic! */ - fifoBuf_putByte(&usart_dev->rx, b); - - /* No error */ - return 0; -} - -/** -* returns number of used bytes in transmit buffer -* \param[in] USART USART name -* \return number of used bytes -* \return 0 if USART not available -*/ -static int32_t PIOS_USART_TxBufferUsed(uint32_t usart_id) -{ - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) - - return (fifoBuf_getUsed(&usart_dev->tx)); -} - -/** -* gets a byte from the transmit buffer -* \param[in] USART USART name -* \return -1 if no new byte available -* \return >= 0: transmitted byte -*/ -static int32_t PIOS_USART_TxBufferGet(uint32_t usart_id) -{ - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) - - if (fifoBuf_getUsed(&usart_dev->tx) == 0) { - /* Nothing new in the buffer */ - return -1; - } - - /* get byte - this operation should be atomic! */ - PIOS_IRQ_Disable(); - uint8_t b = fifoBuf_getByte(&usart_dev->tx); - PIOS_IRQ_Enable(); - - /* Return received byte */ - return b; -} - -/** -* puts more than one byte onto the transmit buffer (used for atomic sends) -* \param[in] USART USART name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if buffer full or cannot get all requested bytes (retry) -*/ -static int32_t PIOS_USART_TxBufferPutMoreNonBlocking(uint32_t usart_id, const uint8_t *buffer, uint16_t len) -{ - struct pios_usart_dev * usart_dev = (struct pios_usart_dev *)usart_id; - - bool valid = PIOS_USART_validate(usart_dev); - PIOS_Assert(valid) - - if (len >= fifoBuf_getFree(&usart_dev->tx)) { - /* Buffer cannot accept all requested bytes (retry) */ - return -1; - } - - /* Copy bytes to be transmitted into transmit buffer */ - /* This operation should be atomic! */ - PIOS_IRQ_Disable(); - - uint16_t used = fifoBuf_getUsed(&usart_dev->tx); - fifoBuf_putData(&usart_dev->tx,buffer,len); - - if (used == 0) { - /* enable sending when buffer was previously empty */ - USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); - } - - PIOS_IRQ_Enable(); - - /* No error */ - return 0; -} - -/** -* puts more than one byte onto the transmit buffer (used for atomic sends)
-* (blocking function) -* \param[in] USART USART name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -*/ -static int32_t PIOS_USART_TxBufferPutMore(uint32_t usart_id, const uint8_t *buffer, uint16_t len) -{ - int32_t rc; - - while ((rc = PIOS_USART_TxBufferPutMoreNonBlocking(usart_id, buffer, len)) == -1); - - return rc; + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usart_dev->tx_out_context = context; + usart_dev->tx_out_cb = tx_out_cb; } static void PIOS_USART_generic_irq_handler(uint32_t usart_id) @@ -413,41 +276,46 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) bool valid = PIOS_USART_validate(usart_dev); PIOS_Assert(valid); - /* Call any user provided callback function instead of processing - * the interrupt ourselves. - */ - if (usart_dev->cfg->irq.handler) { - (usart_dev->cfg->irq.handler)(usart_id); - return; - } - /* Force read of dr after sr to make sure to clear error flags */ volatile uint16_t sr = usart_dev->cfg->regs->SR; volatile uint8_t dr = usart_dev->cfg->regs->DR; /* Check if RXNE flag is set */ + bool rx_need_yield = false; if (sr & USART_SR_RXNE) { - if (PIOS_USART_RxBufferPut(usart_id, dr) < 0) { - /* Here we could add some error handling */ + uint8_t byte = dr; + if (usart_dev->rx_in_cb) { + (void) (usart_dev->rx_in_cb)(usart_dev->rx_in_context, &byte, 1, NULL, &rx_need_yield); } } /* Check if TXE flag is set */ + bool tx_need_yield = false; if (sr & USART_SR_TXE) { - if (PIOS_USART_TxBufferUsed(usart_id) > 0) { - int32_t b = PIOS_USART_TxBufferGet(usart_id); + if (usart_dev->tx_out_cb) { + uint8_t b; + uint16_t bytes_to_send; - if (b < 0) { - /* Here we could add some error handling */ - usart_dev->cfg->regs->DR = 0xff; + bytes_to_send = (usart_dev->tx_out_cb)(usart_dev->tx_out_context, &b, 1, NULL, &tx_need_yield); + + if (bytes_to_send > 0) { + /* Send the byte we've been given */ + usart_dev->cfg->regs->DR = b; } else { - usart_dev->cfg->regs->DR = b & 0xff; + /* No bytes to send, disable TXE interrupt */ + USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); } } else { - /* Disable TXE interrupt (TXEIE=0) */ + /* No bytes to send, disable TXE interrupt */ USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, DISABLE); } } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (rx_need_yield || tx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c index d66b92598..0ebca1320 100644 --- a/flight/PiOS/STM32F10x/pios_usb_hid.c +++ b/flight/PiOS/STM32F10x/pios_usb_hid.c @@ -36,76 +36,106 @@ #include "usb_lib.h" #include "pios_usb_hid_desc.h" #include "stm32f10x.h" -#include "fifo_buffer.h" + +#include "pios_usb_hid_priv.h" #if defined(PIOS_INCLUDE_USB_HID) -#if defined(PIOS_INCLUDE_FREERTOS) -#define USE_FREERTOS -#endif - -static int32_t PIOS_USB_HID_TxBufferPutMoreNonBlocking(uint32_t usbcom_id, const uint8_t *buffer, uint16_t len); -static int32_t PIOS_USB_HID_TxBufferPutMore(uint32_t usbcom_id, const uint8_t *buffer, uint16_t len); -static int32_t PIOS_USB_HID_RxBufferGet(uint32_t usbcom_id); -static int32_t PIOS_USB_HID_RxBufferUsed(uint32_t usbcom_id); +static void PIOS_USB_HID_TxStart(uint32_t usbcom_id, uint16_t tx_bytes_avail); +static void PIOS_USB_HID_RxStart(uint32_t usbcom_id, uint16_t rx_bytes_avail); +static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbcom_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbcom_id, pios_com_callback rx_in_cb, uint32_t context); const struct pios_com_driver pios_usb_com_driver = { - .tx_nb = PIOS_USB_HID_TxBufferPutMoreNonBlocking, - .tx = PIOS_USB_HID_TxBufferPutMore, - .rx = PIOS_USB_HID_RxBufferGet, - .rx_avail = PIOS_USB_HID_RxBufferUsed, + .tx_start = PIOS_USB_HID_TxStart, + .rx_start = PIOS_USB_HID_RxStart, + .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, + .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, }; -// TODO: Eventually replace the transmit and receive buffers with bigger ring bufers -// so there isn't hte 64 byte cap in place by the USB interrupt packet definition +enum pios_usb_hid_dev_magic { + PIOS_USB_HID_DEV_MAGIC = 0xAABBCCDD, +}; + +struct pios_usb_hid_dev { + enum pios_usb_hid_dev_magic magic; + const struct pios_usb_hid_cfg * cfg; + + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + + uint8_t rx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2]; + uint8_t tx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2]; +}; + +static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) +{ + return (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +{ + struct pios_usb_hid_dev * usb_hid_dev; + + usb_hid_dev = (struct pios_usb_hid_dev *)malloc(sizeof(*usb_hid_dev)); + if (!usb_hid_dev) return(NULL); + + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + return(usb_hid_dev); +} +#else +static struct pios_usb_hid_dev pios_usb_hid_devs[PIOS_USB_HID_MAX_DEVS]; +static uint8_t pios_usb_hid_num_devs; +static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) +{ + struct pios_usb_hid_dev * usb_hid_dev; + + if (pios_usb_hid_num_devs >= PIOS_USB_HID_MAX_DEVS) { + return (NULL); + } + + usb_hid_dev = &pios_usb_hid_devs[pios_usb_hid_num_devs++]; + usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; + + return (usb_hid_dev); +} +#endif /* Rx/Tx status */ static uint8_t transfer_possible = 0; -static uint8_t rx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; -static uint8_t tx_packet_buffer[PIOS_USB_HID_DATA_LENGTH + 2] = { 0 }; - -uint8_t rx_pios_fifo_buf[PIOS_USB_RX_BUFFER_SIZE] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement -t_fifo_buffer rx_pios_fifo_buffer; - -uint8_t tx_pios_fifo_buf[PIOS_USB_TX_BUFFER_SIZE] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement -t_fifo_buffer tx_pios_fifo_buffer; - -#if defined(USE_FREERTOS) -xSemaphoreHandle pios_usb_tx_semaphore; -#endif /** * Initialises USB COM layer - * \param[in] mode currently only mode 0 supported * \return < 0 if initialisation failed * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions */ -int32_t PIOS_USB_HID_Init(uint32_t mode) +static uint32_t pios_usb_hid_id; +int32_t PIOS_USB_HID_Init(uint32_t * usb_hid_id, const struct pios_usb_hid_cfg * cfg) { - /* Currently only mode 0 supported */ - if (mode != 0) { - /* Unsupported mode */ - return -1; - } + PIOS_Assert(usb_hid_id); + PIOS_Assert(cfg); - fifoBuf_init(&rx_pios_fifo_buffer, rx_pios_fifo_buf, sizeof(rx_pios_fifo_buf)); - fifoBuf_init(&tx_pios_fifo_buffer, tx_pios_fifo_buf, sizeof(tx_pios_fifo_buf)); + struct pios_usb_hid_dev * usb_hid_dev; + + usb_hid_dev = (struct pios_usb_hid_dev *) PIOS_USB_HID_alloc(); + if (!usb_hid_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + usb_hid_dev->cfg = cfg; PIOS_USB_HID_Reenumerate(); - /* Create semaphore before enabling interrupts */ -#if defined(USE_FREERTOS) - vSemaphoreCreateBinary(pios_usb_tx_semaphore); -#endif - + /* + * This is a horrible hack to make this available to + * the interrupt callbacks. This should go away ASAP. + */ + pios_usb_hid_id = (uint32_t) usb_hid_dev; + /* Enable the USB Interrupts */ - /* 2 bit for pre-emption priority, 2 bits for subpriority */ - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + NVIC_Init(&usb_hid_dev->cfg->irq.init); /* Select USBCLK source */ RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5); @@ -122,7 +152,12 @@ int32_t PIOS_USB_HID_Init(uint32_t mode) USB_Init(); USB_SIL_Init(); + *usb_hid_id = (uint32_t) usb_hid_dev; + return 0; /* No error */ + +out_fail: + return(-1); } /** @@ -201,127 +236,118 @@ int32_t PIOS_USB_HID_CheckAvailable(uint8_t id) return (PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) != 0 && transfer_possible ? 1 : 0; } -void sendChunk() +static void PIOS_USB_HID_SendReport(struct pios_usb_hid_dev * usb_hid_dev) { + uint16_t bytes_to_tx; - uint32_t size = fifoBuf_getUsed(&tx_pios_fifo_buffer); - if ((size > 0) && (GetEPTxStatus(ENDP1) != EP_TX_VALID)) { - if (size > PIOS_USB_HID_DATA_LENGTH) - size = PIOS_USB_HID_DATA_LENGTH; + if (!usb_hid_dev->tx_out_cb) { + return; + } + + bool need_yield = false; #ifdef USB_HID - fifoBuf_getData(&tx_pios_fifo_buffer, &tx_packet_buffer[1], size + 1); - tx_packet_buffer[0] = 1; /* report ID */ + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[1], + sizeof(usb_hid_dev->tx_packet_buffer)-1, + NULL, + &need_yield); #else - fifoBuf_getData(&tx_pios_fifo_buffer, &tx_packet_buffer[2], size); - tx_packet_buffer[0] = 1; /* report ID */ - tx_packet_buffer[1] = size; /* valid data length */ - + bytes_to_tx = (usb_hid_dev->tx_out_cb)(usb_hid_dev->tx_out_context, + &usb_hid_dev->tx_packet_buffer[2], + sizeof(usb_hid_dev->tx_packet_buffer)-2, + NULL, + &need_yield); #endif - - UserToPMABufferCopy((uint8_t *) tx_packet_buffer, GetEPTxAddr(EP1_IN & 0x7F), size + 2); - SetEPTxCount((EP1_IN & 0x7F), PIOS_USB_HID_DATA_LENGTH + 2); - - /* Send Buffer */ - SetEPTxValid(ENDP1); + if (bytes_to_tx == 0) { + return; } -} + /* Always set type as report ID */ + usb_hid_dev->tx_packet_buffer[0] = 1; -/** - * Puts more than one byte onto the transmit buffer (used for atomic sends) - * \param[in] *buffer pointer to buffer which should be transmitted - * \param[in] len number of bytes which should be transmitted - * \return 0 if no error - * \return -1 if port unavailable (disconnected) - * \return -2 if too many bytes to be send - * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions - */ -static int32_t PIOS_USB_HID_TxBufferPutMoreNonBlocking(uint32_t usbcom_id, const uint8_t * buffer, uint16_t len) -{ - uint16_t ret; - - if(!transfer_possible) - return -1; - - if (len > fifoBuf_getFree(&tx_pios_fifo_buffer)) { - sendChunk(); /* Try and send what's in the buffer though */ - return -2; /* Cannot send all requested bytes */ - } - - /* don't check returned bytes because it should always succeed */ - /* after previous thread and no meaningful way to deal with the */ - /* case it only buffers half the bytes */ -#if defined(USE_FREERTOS) - if(!xSemaphoreTake(pios_usb_tx_semaphore,10 / portTICK_RATE_MS)) - return -3; +#ifdef USB_HID + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, GetEPTxAddr(EP1_IN & 0x7F), bytes_to_tx + 1); +#else + usb_hid_dev->tx_packet_buffer[1] = bytes_to_tx; + UserToPMABufferCopy(usb_hid_dev->tx_packet_buffer, GetEPTxAddr(EP1_IN & 0x7F), bytes_to_tx + 2); #endif + /* Is this correct? Why do we always send the whole buffer? */ + SetEPTxCount((EP1_IN & 0x7F), sizeof(usb_hid_dev->tx_packet_buffer)); + SetEPTxValid(ENDP1); - ret = fifoBuf_putData(&tx_pios_fifo_buffer, buffer, len); - -#if defined(USE_FREERTOS) - xSemaphoreGive(pios_usb_tx_semaphore); -#endif - - sendChunk(); - - return 0; -} - -/** - * Puts more than one byte onto the transmit buffer (used for atomic sends)
- * (Blocking Function) - * \param[in] *buffer pointer to buffer which should be transmitted - * \param[in] len number of bytes which should be transmitted - * \return 0 if no error - * \return -1 if too many bytes to be send - * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions - */ -static int32_t PIOS_USB_HID_TxBufferPutMore(uint32_t usbcom_id, const uint8_t *buffer, uint16_t len) -{ - if(len > (fifoBuf_getUsed(&tx_pios_fifo_buffer) + fifoBuf_getFree(&tx_pios_fifo_buffer))) - return -1; - - uint32_t error; - while ((error = PIOS_USB_HID_TxBufferPutMoreNonBlocking(usbcom_id, buffer, len)) == -2) { #if defined(PIOS_INCLUDE_FREERTOS) - taskYIELD(); -#endif + if (need_yield) { + vPortYieldFromISR(); } - - return error; +#endif /* PIOS_INCLUDE_FREERTOS */ } -/** - * Gets a byte from the receive buffer - * \return -1 if no new byte available - * \return >= 0: received byte - * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions - */ -static int32_t PIOS_USB_HID_RxBufferGet(uint32_t usbcom_id) -{ - uint8_t read; +static void PIOS_USB_HID_RxStart(uint32_t usbcom_id, uint16_t rx_bytes_avail) { + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbcom_id; - if(fifoBuf_getUsed(&rx_pios_fifo_buffer) == 0) - return -1; + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); - read = fifoBuf_getByte(&rx_pios_fifo_buffer); + if (!transfer_possible) { + return; + } // If endpoint was stalled and there is now space make it valid - if ((GetEPRxStatus(ENDP1) != EP_RX_VALID) && (fifoBuf_getFree(&rx_pios_fifo_buffer) > 62)) { + PIOS_IRQ_Disable(); + if ((GetEPRxStatus(ENDP1) != EP_RX_VALID) && + (rx_bytes_avail > PIOS_USB_HID_DATA_LENGTH)) { SetEPRxStatus(ENDP1, EP_RX_VALID); } - return read; + PIOS_IRQ_Enable(); } -/** - * Returns number of used bytes in receive buffer - * \return > 0: number of used bytes - * \return 0 nothing available - * \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions - */ -static int32_t PIOS_USB_HID_RxBufferUsed(uint32_t usbcom_id) +static void PIOS_USB_HID_TxStart(uint32_t usbcom_id, uint16_t tx_bytes_avail) { - return fifoBuf_getUsed(&rx_pios_fifo_buffer); + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbcom_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); + + if (!transfer_possible) { + return; + } + + if (GetEPTxStatus(ENDP1) == EP_TX_VALID) { + /* Endpoint is already transmitting */ + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); +} + +static void PIOS_USB_HID_RegisterRxCallback(uint32_t usbcom_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbcom_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->rx_in_context = context; + usb_hid_dev->rx_in_cb = rx_in_cb; +} + +static void PIOS_USB_HID_RegisterTxCallback(uint32_t usbcom_id, pios_com_callback tx_out_cb, uint32_t context) +{ + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)usbcom_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + usb_hid_dev->tx_out_context = context; + usb_hid_dev->tx_out_cb = tx_out_cb; } /** @@ -330,7 +356,16 @@ static int32_t PIOS_USB_HID_RxBufferUsed(uint32_t usbcom_id) */ void PIOS_USB_HID_EP1_IN_Callback(void) { - sendChunk(); + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); + + if (!transfer_possible) { + return; + } + + PIOS_USB_HID_SendReport(usb_hid_dev); } /** @@ -338,28 +373,58 @@ void PIOS_USB_HID_EP1_IN_Callback(void) */ void PIOS_USB_HID_EP1_OUT_Callback(void) { + struct pios_usb_hid_dev * usb_hid_dev = (struct pios_usb_hid_dev *)pios_usb_hid_id; + + bool valid = PIOS_USB_HID_validate(usb_hid_dev); + PIOS_Assert(valid); + uint32_t DataLength = 0; /* Read received data (63 bytes) */ /* Get the number of received data on the selected Endpoint */ DataLength = GetEPRxCount(ENDP1 & 0x7F); + if (DataLength > sizeof(usb_hid_dev->rx_packet_buffer)) { + DataLength = sizeof(usb_hid_dev->rx_packet_buffer); + } /* Use the memory interface function to write to the selected endpoint */ - PMAToUserBufferCopy((uint8_t *) &rx_packet_buffer[0], GetEPRxAddr(ENDP1 & 0x7F), DataLength); + PMAToUserBufferCopy((uint8_t *) usb_hid_dev->rx_packet_buffer, GetEPRxAddr(ENDP1 & 0x7F), DataLength); + + if (!usb_hid_dev->rx_in_cb) { + /* No Rx call back registered, disable the receiver */ + SetEPRxStatus(ENDP1, EP_RX_NAK); + return; + } /* The first byte is report ID (not checked), the second byte is the valid data length */ + uint16_t headroom; + bool need_yield = false; #ifdef USB_HID - fifoBuf_putData(&rx_pios_fifo_buffer, &rx_packet_buffer[1], PIOS_USB_HID_DATA_LENGTH + 1); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[1], + sizeof(usb_hid_dev->rx_packet_buffer)-1, + &headroom, + &need_yield); #else - fifoBuf_putData(&rx_pios_fifo_buffer, &rx_packet_buffer[2], rx_packet_buffer[1]); + (usb_hid_dev->rx_in_cb)(usb_hid_dev->rx_in_context, + &usb_hid_dev->rx_packet_buffer[2], + usb_hid_dev->rx_packet_buffer[1], + &headroom, + &need_yield); #endif - - // Only reactivate endpoint if available space in buffer - if (fifoBuf_getFree(&rx_pios_fifo_buffer) > 62) { + if (headroom > PIOS_USB_HID_DATA_LENGTH) { + /* We have room for a maximum length message */ SetEPRxStatus(ENDP1, EP_RX_VALID); } else { + /* Not enough room left for a message, apply backpressure */ SetEPRxStatus(ENDP1, EP_RX_NAK); } + +#if defined(PIOS_INCLUDE_FREERTOS) + if (need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ } #endif diff --git a/flight/PiOS/inc/pios_com.h b/flight/PiOS/inc/pios_com.h index 0af333d7b..cbee33735 100644 --- a/flight/PiOS/inc/pios_com.h +++ b/flight/PiOS/inc/pios_com.h @@ -32,17 +32,19 @@ #ifndef PIOS_COM_H #define PIOS_COM_H +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); + struct pios_com_driver { - void (*init)(uint32_t id); - void (*set_baud)(uint32_t id, uint32_t baud); - int32_t (*tx_nb)(uint32_t id, const uint8_t *buffer, uint16_t len); - int32_t (*tx)(uint32_t id, const uint8_t *buffer, uint16_t len); - int32_t (*rx)(uint32_t id); - int32_t (*rx_avail)(uint32_t id); + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); }; /* Public Functions */ -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, const uint32_t lower_id); +extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); @@ -52,7 +54,7 @@ extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); -extern uint8_t PIOS_COM_ReceiveBuffer(uint32_t com_id); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id); #endif /* PIOS_COM_H */ diff --git a/flight/PiOS/inc/pios_com_priv.h b/flight/PiOS/inc/pios_com_priv.h index 2bf58a300..54af82bcb 100644 --- a/flight/PiOS/inc/pios_com_priv.h +++ b/flight/PiOS/inc/pios_com_priv.h @@ -34,16 +34,6 @@ #include -enum pios_com_dev_magic { - PIOS_COM_DEV_MAGIC = 0xaa55aa55, -}; - -struct pios_com_dev { - enum pios_com_dev_magic magic; - uint32_t id; - const struct pios_com_driver * driver; -}; - extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id); #endif /* PIOS_COM_PRIV_H */ diff --git a/flight/PiOS/inc/pios_delay.h b/flight/PiOS/inc/pios_delay.h index 647e9f553..17e6d0452 100644 --- a/flight/PiOS/inc/pios_delay.h +++ b/flight/PiOS/inc/pios_delay.h @@ -34,10 +34,10 @@ /* Public Functions */ extern int32_t PIOS_DELAY_Init(void); -extern int32_t PIOS_DELAY_WaituS(uint16_t uS); -extern int32_t PIOS_DELAY_WaitmS(uint16_t mS); -extern uint16_t PIOS_DELAY_GetuS(); -extern int32_t PIOS_DELAY_DiffuS(uint16_t ref); +extern int32_t PIOS_DELAY_WaituS(uint32_t uS); +extern int32_t PIOS_DELAY_WaitmS(uint32_t mS); +extern uint32_t PIOS_DELAY_GetuS(); +extern uint32_t PIOS_DELAY_GetuSSince(uint32_t t); #endif /* PIOS_DELAY_H */ diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index a32160894..dfec004f5 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -39,8 +39,8 @@ struct pios_rcvr_channel_map { extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; struct pios_rcvr_driver { - void (*init)(uint32_t id); - int32_t (*read)(uint32_t id, uint8_t channel); + void (*init)(uint32_t id); + int32_t (*read)(uint32_t id, uint8_t channel); }; /* Public Functions */ diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index 7b880a2c8..4e46b2089 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -75,11 +75,9 @@ struct pios_sbus_cfg { BitAction gpio_inv_enable; }; -extern void PIOS_SBUS_irq_handler(); - extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; -extern void PIOS_SBUS_Init(const struct pios_sbus_cfg * cfg); +extern int32_t PIOS_SBUS_Init(uint32_t * sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id); #endif /* PIOS_SBUS_PRIV_H */ diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index 083df8fd3..2d31a8027 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -40,11 +40,9 @@ struct pios_spektrum_cfg { uint32_t remap; /* GPIO_Remap_* */ }; -extern void PIOS_SPEKTRUM_irq_handler(); - extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; -extern void PIOS_SPEKTRUM_Init(const struct pios_spektrum_cfg * cfg, bool bind); +extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind); #endif /* PIOS_PWM_PRIV_H */ diff --git a/flight/PiOS/inc/pios_stm32.h b/flight/PiOS/inc/pios_stm32.h index 3ea6e1f95..fb9163208 100644 --- a/flight/PiOS/inc/pios_stm32.h +++ b/flight/PiOS/inc/pios_stm32.h @@ -32,7 +32,6 @@ #define PIOS_STM32_H struct stm32_irq { - void (*handler) (uint32_t); uint32_t flags; NVIC_InitTypeDef init; }; diff --git a/flight/PiOS/inc/pios_usb_hid.h b/flight/PiOS/inc/pios_usb_hid.h index 06f4deca1..67067ba72 100644 --- a/flight/PiOS/inc/pios_usb_hid.h +++ b/flight/PiOS/inc/pios_usb_hid.h @@ -41,7 +41,6 @@ #define PIOS_USB_HID_DATA_LENGTH 62 /* Global functions */ -extern int32_t PIOS_USB_HID_Init(uint32_t mode); extern int32_t PIOS_USB_HID_Reenumerate(); extern int32_t PIOS_USB_HID_ChangeConnectionState(uint32_t Connected); extern int32_t PIOS_USB_HID_CheckAvailable(uint8_t id); diff --git a/flight/PiOS/inc/pios_usb_hid_priv.h b/flight/PiOS/inc/pios_usb_hid_priv.h new file mode 100644 index 000000000..5a8652762 --- /dev/null +++ b/flight/PiOS/inc/pios_usb_hid_priv.h @@ -0,0 +1,49 @@ + /** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_HID USB_HID Functions + * @brief PIOS interface for USB_HID port + * @{ + * + * @file pios_usb_hid_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief USB_HID private 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_HID_PRIV_H +#define PIOS_USB_HID_PRIV_H + +#include +#include + +struct pios_usb_hid_cfg { + struct stm32_irq irq; +}; + +extern int32_t PIOS_USB_HID_Init(uint32_t * usb_hid_id, const struct pios_usb_hid_cfg * cfg); + +#endif /* PIOS_USB_HID_PRIV_H */ + +/** + * @} + * @} + */ + diff --git a/flight/PipXtreme/api_config.c b/flight/PipXtreme/api_config.c index bf10a13b3..2cecfc65e 100644 --- a/flight/PipXtreme/api_config.c +++ b/flight/PipXtreme/api_config.c @@ -794,7 +794,8 @@ void apiconfig_process(void) int32_t bytes = PIOS_COM_ReceiveBufferUsed(PIOS_COM_SERIAL); while (bytes > 0) { - PIOS_COM_ReceiveBuffer(PIOS_COM_SERIAL); + uint8_t c; + PIOS_COM_ReceiveBuffer(PIOS_COM_SERIAL, &c, 1, 0); bytes--; } } @@ -812,9 +813,12 @@ void apiconfig_process(void) if (com_num > sizeof(apiconfig_rx_buffer) - apiconfig_rx_buffer_wr) com_num = sizeof(apiconfig_rx_buffer) - apiconfig_rx_buffer_wr; + while (com_num > 0) { // fetch a byte from the comm-port RX buffer and save it into our RX buffer - apiconfig_rx_buffer[apiconfig_rx_buffer_wr++] = PIOS_COM_ReceiveBuffer(apiconfig_comm_port); + uint8_t c; + PIOS_COM_ReceiveBuffer(apiconfig_comm_port, &c, 1, 0); + apiconfig_rx_buffer[apiconfig_rx_buffer_wr++] = c; com_num--; } diff --git a/flight/PipXtreme/main.c b/flight/PipXtreme/main.c index f30a355a8..46b9073ea 100644 --- a/flight/PipXtreme/main.c +++ b/flight/PipXtreme/main.c @@ -163,8 +163,8 @@ void TIMER_INT_FUNC(void) // Clear timer interrupt pending bit TIM_ClearITPendingBit(TIMER_INT_TIMER, TIM_IT_Update); -// random32 = UpdateCRC32(random32, PIOS_DELAY_TIMER->CNT >> 8); -// random32 = UpdateCRC32(random32, PIOS_DELAY_TIMER->CNT); +// random32 = UpdateCRC32(random32, PIOS_DELAY_GetuS() >> 8); +// random32 = UpdateCRC32(random32, PIOS_DELAY_GetuS()); uptime_ms++; @@ -785,8 +785,8 @@ int main() for (;;) { - random32 = updateCRC32(random32, PIOS_DELAY_TIMER->CNT >> 8); - random32 = updateCRC32(random32, PIOS_DELAY_TIMER->CNT); + random32 = updateCRC32(random32, PIOS_DELAY_GetuS() >> 8); + random32 = updateCRC32(random32, PIOS_DELAY_GetuS()); if (second_tick) { diff --git a/flight/PipXtreme/pios_board.c b/flight/PipXtreme/pios_board.c index 5bbca2f9f..0a69808ad 100644 --- a/flight/PipXtreme/pios_board.c +++ b/flight/PipXtreme/pios_board.c @@ -90,7 +90,6 @@ static const struct pios_spi_cfg pios_spi_port_cfg = .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), .init = { .NVIC_IRQChannel = DMA1_Channel2_IRQn, @@ -193,7 +192,6 @@ static const struct pios_adc_cfg pios_adc_cfg = { .dma = { .ahb_clk = RCC_AHBPeriph_DMA1, .irq = { - .handler = NULL, .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), .init = { .NVIC_IRQChannel = DMA1_Channel1_IRQn, @@ -259,7 +257,6 @@ static const struct pios_usart_cfg pios_usart_serial_cfg = }, .irq = { - .handler = NULL, .init = { .NVIC_IRQChannel = USART1_IRQn, @@ -298,10 +295,37 @@ static const struct pios_usart_cfg pios_usart_serial_cfg = #include +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 + +static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; +static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; + +#define PIOS_COM_SERIAL_RX_BUF_LEN 192 +#define PIOS_COM_SERIAL_TX_BUF_LEN 192 + +static uint8_t pios_com_serial_rx_buffer[PIOS_COM_SERIAL_RX_BUF_LEN]; +static uint8_t pios_com_serial_tx_buffer[PIOS_COM_SERIAL_TX_BUF_LEN]; + #endif /* PIOS_INCLUDE_COM */ // *********************************************************************************** +#if defined(PIOS_INCLUDE_USB_HID) +#include "pios_usb_hid_priv.h" + +static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_serial_id; @@ -329,18 +353,23 @@ void PIOS_Board_Init(void) { if (PIOS_USART_Init(&pios_usart_serial_id, &pios_usart_serial_cfg)) { PIOS_DEBUG_Assert(0); } - if (PIOS_COM_Init(&pios_com_serial_id, &pios_usart_com_driver, pios_usart_serial_id)) { + if (PIOS_COM_Init(&pios_com_serial_id, &pios_usart_com_driver, pios_usart_serial_id, + pios_com_serial_rx_buffer, sizeof(pios_com_serial_rx_buffer), + pios_com_serial_tx_buffer, sizeof(pios_com_serial_tx_buffer))) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); + uint32_t pios_usb_hid_id; + PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { - PIOS_DEBUG_Assert(0); + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id, + pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), + pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_COM */ -#endif /* PIOS_INCLUDE_USB_HID */ +#endif /* PIOS_INCLUDE_USB_HID */ // ADC system // PIOS_ADC_Init(); diff --git a/flight/PipXtreme/transparent_comms.c b/flight/PipXtreme/transparent_comms.c index a04ca17ee..b30f5f581 100644 --- a/flight/PipXtreme/transparent_comms.c +++ b/flight/PipXtreme/transparent_comms.c @@ -91,7 +91,8 @@ void trans_process(void) int32_t bytes = PIOS_COM_ReceiveBufferUsed(PIOS_COM_SERIAL); while (bytes > 0) { - PIOS_COM_ReceiveBuffer(PIOS_COM_SERIAL); + uint8_t c; + PIOS_COM_ReceiveBuffer(PIOS_COM_SERIAL, &c, 1, 0); bytes--; } } @@ -134,8 +135,7 @@ void trans_process(void) // copy data received down the comm-port into our temp buffer register uint16_t bytes_saved = 0; - while (bytes_saved < com_num) - trans_temp_buffer1[bytes_saved++] = PIOS_COM_ReceiveBuffer(comm_port); + bytes_saved = PIOS_COM_ReceiveBuffer(comm_port, trans_temp_buffer1, com_num, 0); // put the received comm-port data bytes into the RF packet handler TX buffer if (bytes_saved > 0) @@ -147,8 +147,11 @@ void trans_process(void) else { // empty the comm-ports rx buffer int32_t com_num = PIOS_COM_ReceiveBufferUsed(comm_port); - while (com_num > 0) - PIOS_COM_ReceiveBuffer(comm_port); + while (com_num > 0) { + uint8_t c; + PIOS_COM_ReceiveBuffer(comm_port, &c, 1, 0); + com_num--; + } } // ******************** diff --git a/flight/Project/gdb/bl_coptercontrol b/flight/Project/gdb/bl_coptercontrol index 52411d9bd..45bb50412 100644 --- a/flight/Project/gdb/bl_coptercontrol +++ b/flight/Project/gdb/bl_coptercontrol @@ -1,7 +1,7 @@ define connect target remote localhost:3333 monitor cortex_m3 vector_catch all - file ./build/bl_coptercontrol/CopterControl_BL.elf + file ./build/bl_coptercontrol/bl_coptercontrol.elf end #monitor reset halt diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 7dfc71545..b9da7f671 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -19,6 +19,19 @@ defineReplace(addNewline) { return($$escape_expand(\\n\\t)) } +defineReplace(qtLibraryName) { + unset(LIBRARY_NAME) + LIBRARY_NAME = $$1 + CONFIG(debug, debug|release) { + !debug_and_release|build_pass { + mac:RET = $$member(LIBRARY_NAME, 0)_debug + else:win32:RET = $$member(LIBRARY_NAME, 0)d + } + } + isEmpty(RET):RET = $$LIBRARY_NAME + return($$RET) +} + # For use in custom compilers which just copy files win32:i_flag = i defineReplace(stripSrcDir) { diff --git a/ground/openpilotgcs/src/app/app.pro b/ground/openpilotgcs/src/app/app.pro index 4b1534ad9..c644f7c97 100644 --- a/ground/openpilotgcs/src/app/app.pro +++ b/ground/openpilotgcs/src/app/app.pro @@ -11,16 +11,15 @@ SOURCES += main.cpp include(../rpath.pri) include(../libs/utils/utils.pri) -win32 { - CONFIG(debug, debug|release):LIBS *= -lExtensionSystemd -lAggregationd -lQExtSerialPortd - else:LIBS *= -lExtensionSystem -lAggregation -lQExtSerialPort +LIBS *= -l$$qtLibraryName(ExtensionSystem) -l$$qtLibraryName(Aggregation) +win32 { +# CONFIG(debug, debug|release):LIBS *= -lExtensionSystemd -lAggregationd -lQExtSerialPortd +# else:LIBS *= -lExtensionSystem -lAggregation -lQExtSerialPort RC_FILE = openpilotgcs.rc target.path = /bin INSTALLS += target } else:macx { - CONFIG(debug, debug|release):LIBS *= -lExtensionSystem_debug -lAggregation_debug - else:LIBS *= -lExtensionSystem -lAggregation LIBS += -framework CoreFoundation ICON = openpilotgcs.icns QMAKE_INFO_PLIST = Info.plist @@ -28,8 +27,6 @@ win32 { FILETYPES.path = Contents/Resources QMAKE_BUNDLE_DATA += FILETYPES } else { - LIBS *= -lExtensionSystem -lAggregation - target.path = /bin INSTALLS += target } diff --git a/ground/openpilotgcs/src/libs/aggregation/aggregation.pri b/ground/openpilotgcs/src/libs/aggregation/aggregation.pri index a6c48c59c..fc6ddc81f 100644 --- a/ground/openpilotgcs/src/libs/aggregation/aggregation.pri +++ b/ground/openpilotgcs/src/libs/aggregation/aggregation.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(Aggregation) +LIBS *= -l$$qtLibraryName(Aggregation) diff --git a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem.pri b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem.pri index 43855eb16..4c7641b9c 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem.pri +++ b/ground/openpilotgcs/src/libs/extensionsystem/extensionsystem.pri @@ -1,3 +1,3 @@ include(extensionsystem_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(ExtensionSystem) +LIBS *= -l$$qtLibraryName(ExtensionSystem) diff --git a/ground/openpilotgcs/src/libs/glc_lib/glc_ext.h b/ground/openpilotgcs/src/libs/glc_lib/glc_ext.h index a9a36f227..6bdd6532d 100644 --- a/ground/openpilotgcs/src/libs/glc_lib/glc_ext.h +++ b/ground/openpilotgcs/src/libs/glc_lib/glc_ext.h @@ -30,6 +30,11 @@ // Buffer offset used by VBO #define BUFFER_OFFSET(i) ((char*)NULL + (i)) +#if defined(Q_OS_MAC) +#include "gl.h" +#include "glu.h" +#endif + #if !defined(Q_OS_MAC) // ARB_vertex_buffer_object extern PFNGLBINDBUFFERARBPROC glBindBuffer; diff --git a/ground/openpilotgcs/src/libs/glc_lib/glc_lib.pri b/ground/openpilotgcs/src/libs/glc_lib/glc_lib.pri index 58610bcaa..d3199ba3a 100644 --- a/ground/openpilotgcs/src/libs/glc_lib/glc_lib.pri +++ b/ground/openpilotgcs/src/libs/glc_lib/glc_lib.pri @@ -1,2 +1,2 @@ QT += opengl -LIBS += -l$$qtLibraryTarget(GLC_lib) +LIBS *= -l$$qtLibraryName(GLC_lib) diff --git a/ground/openpilotgcs/src/libs/glc_lib/viewport/glc_camera.cpp b/ground/openpilotgcs/src/libs/glc_lib/viewport/glc_camera.cpp index 05eba540f..7229178bc 100644 --- a/ground/openpilotgcs/src/libs/glc_lib/viewport/glc_camera.cpp +++ b/ground/openpilotgcs/src/libs/glc_lib/viewport/glc_camera.cpp @@ -25,6 +25,11 @@ #include "glc_camera.h" +#if defined(Q_OS_MAC) +#include "gl.h" +#include "glu.h" +#endif + #include using namespace glc; diff --git a/ground/openpilotgcs/src/libs/libqxt/libqxt.pri b/ground/openpilotgcs/src/libs/libqxt/libqxt.pri index d2001f6fe..1886943d5 100644 --- a/ground/openpilotgcs/src/libs/libqxt/libqxt.pri +++ b/ground/openpilotgcs/src/libs/libqxt/libqxt.pri @@ -1,4 +1,4 @@ -LIBS += -l$$qtLibraryTarget(QxtCore) +LIBS *= -l$$qtLibraryName(QxtCore) INCLUDEPATH += \ $$GCS_SOURCE_TREE/src/libs/libqxt/src/core diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pri b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pri index 9c0188c30..f6ea6004b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pri +++ b/ground/openpilotgcs/src/libs/opmapcontrol/opmapcontrol.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(opmapwidget) +LIBS *= -l$$qtLibraryName(opmapwidget) diff --git a/ground/openpilotgcs/src/libs/qextserialport/qextserialport.pri b/ground/openpilotgcs/src/libs/qextserialport/qextserialport.pri index 1f89982c5..286b70b87 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/qextserialport.pri +++ b/ground/openpilotgcs/src/libs/qextserialport/qextserialport.pri @@ -1,2 +1,2 @@ -LIBS += -l$$qtLibraryTarget(QExtSerialPort) +LIBS *= -l$$qtLibraryName(QExtSerialPort) diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h index 60d973f41..8d3ac29a6 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h +++ b/ground/openpilotgcs/src/libs/qextserialport/src/qextserialport.h @@ -135,6 +135,7 @@ struct PortSettings #include #include #elif (defined Q_OS_WIN) +#include #include #include #include @@ -308,6 +309,9 @@ class QEXTSERIALPORT_EXPORT QextSerialPort: public QIODevice #ifdef Q_OS_WIN private slots: void onWinEvent(HANDLE h); + void triggerTxEmpty(); +private: + QTimer fakeTxEmpty; #endif private: diff --git a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp index 350dc7533..1fe0ff3ba 100644 --- a/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp +++ b/ground/openpilotgcs/src/libs/qextserialport/src/win_qextserialport.cpp @@ -53,7 +53,6 @@ bool QextSerialPort::open(OpenMode mode) { DWORD dwFlagsAndAttributes = 0; if (queryMode() == QextSerialPort::EventDriven) dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; - QMutexLocker lock(mutex); if (mode == QIODevice::NotOpen) return isOpen(); @@ -95,7 +94,10 @@ bool QextSerialPort::open(OpenMode mode) { } winEventNotifier = new QWinEventNotifier(overlap.hEvent, this); connect(winEventNotifier, SIGNAL(activated(HANDLE)), this, SLOT(onWinEvent(HANDLE))); + connect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); + fakeTxEmpty.start(10000); WaitCommEvent(Win_Handle, &eventMask, &overlap); + } } } else { @@ -111,8 +113,10 @@ is not currently open. void QextSerialPort::close() { QMutexLocker lock(mutex); + fakeTxEmpty.stop(); + disconnect(&fakeTxEmpty,SIGNAL(timeout()),this,SLOT(triggerTxEmpty())); if (isOpen()) { - flush(); + flush(); QIODevice::close(); // mark ourselves as closed CancelIo(Win_Handle); if (CloseHandle(Win_Handle)) @@ -886,4 +890,38 @@ void QextSerialPort::setTimeout(long millisec) { if (queryMode() != QextSerialPort::EventDriven) SetCommTimeouts(Win_Handle, &Win_CommTimeouts); } +/*! +emulates the EV_TXEMPTY system event not present on some BT interfaces +*/ +void QextSerialPort::triggerTxEmpty() +{ + if (bytesToWrite()>500) + { + QMutexLocker lock(mutex); + qint64 totalBytesWritten = 0; + QList overlapsToDelete; + foreach(OVERLAPPED* o, pendingWrites) { + DWORD numBytes = 0; + if (GetOverlappedResult(Win_Handle, o, & numBytes, false)) { + overlapsToDelete.append(o); + totalBytesWritten += numBytes; + } else if( GetLastError() != ERROR_IO_INCOMPLETE ) { + overlapsToDelete.append(o); + qWarning() << "CommEvent overlapped write error:" << GetLastError(); + } + } + if (totalBytesWritten > 0) { + QWriteLocker writelocker(bytesToWriteLock); + _bytesToWrite = 0; + //qDebug()<<"zeroed bytesToWrite"; + } + //qDebug()<<"overlapsToDelete"<hEvent); + delete toDelete; + } + } + +} diff --git a/ground/openpilotgcs/src/libs/qscispinbox/qscispinbox.pri b/ground/openpilotgcs/src/libs/qscispinbox/qscispinbox.pri index 3d63b6605..85cd57253 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/qscispinbox.pri +++ b/ground/openpilotgcs/src/libs/qscispinbox/qscispinbox.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(QScienceSpinBox) +LIBS *= -l$$qtLibraryName(QScienceSpinBox) diff --git a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent.pri b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent.pri index 57929a4cf..141de8ee8 100644 --- a/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent.pri +++ b/ground/openpilotgcs/src/libs/qtconcurrent/qtconcurrent.pri @@ -1 +1 @@ -LIBS *= -l$$qtLibraryTarget(QtConcurrent) +LIBS *= -l$$qtLibraryName(QtConcurrent) diff --git a/ground/openpilotgcs/src/libs/qwt/qwt.pri b/ground/openpilotgcs/src/libs/qwt/qwt.pri index 4241b6278..7d57a8ce7 100644 --- a/ground/openpilotgcs/src/libs/qwt/qwt.pri +++ b/ground/openpilotgcs/src/libs/qwt/qwt.pri @@ -1,2 +1,2 @@ -LIBS += -l$$qtLibraryTarget(Qwt) +LIBS *= -l$$qtLibraryName(Qwt) diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pri b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pri index 01fd85523..a5d13203c 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pri +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.pri @@ -1 +1 @@ -LIBS += -l$$qtLibraryTarget(sdlgamepad) +LIBS *= -l$$qtLibraryName(sdlgamepad) diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp new file mode 100644 index 000000000..f5fbfb639 --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.cpp @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * + * @file mylistwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 "mylistwidget.h" + +QStyleOptionViewItem MyListWidget::viewOptions() const +{ + QStyleOptionViewItem option = QListWidget::viewOptions(); + if (m_iconAbove) { + option.decorationPosition = QStyleOptionViewItem::Top; + option.displayAlignment = Qt::AlignCenter; + } + return option; +} diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.h b/ground/openpilotgcs/src/libs/utils/mylistwidget.h new file mode 100644 index 000000000..40059927e --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * + * @file mylistwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 MYLISTWIDGET_H +#define MYLISTWIDGET_H + +#include "utils_global.h" + +#include + +/* + * MyListWidget is a plain QListWidget but with the added option + * to place the icon above the label in ListMode. This is achieved + * the easiest by subclassing QListWidget and overriding viewOptions(). + */ +class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget +{ + Q_OBJECT +public: + MyListWidget(QWidget *parent) : QListWidget(parent), m_iconAbove(false) { } + void setIconAbove(bool iconAbove) { m_iconAbove = iconAbove; } +protected: + QStyleOptionViewItem viewOptions() const; +private: + bool m_iconAbove; +}; + +#endif // MYLISTWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp new file mode 100644 index 000000000..faf2c911e --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * + * @file mytabbedstackwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 "mytabbedstackwidget.h" +#include +#include +#include +#include + +MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool iconAbove) + : QWidget(parent), + m_vertical(isVertical), + m_iconAbove(iconAbove) +{ + m_listWidget = new MyListWidget(this); + m_listWidget->setIconAbove(m_iconAbove); + m_stackWidget = new QStackedWidget(); + m_stackWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + QBoxLayout *toplevelLayout; + if (m_vertical) { + toplevelLayout = new QHBoxLayout; + toplevelLayout->addWidget(m_listWidget); + toplevelLayout->addWidget(m_stackWidget); + m_listWidget->setFlow(QListView::TopToBottom); + m_listWidget->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); + } else { + toplevelLayout = new QVBoxLayout; + toplevelLayout->addWidget(m_stackWidget); + toplevelLayout->addWidget(m_listWidget); + m_listWidget->setFlow(QListView::LeftToRight); + m_listWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + } + if (m_iconAbove && m_vertical) + m_listWidget->setFixedWidth(90); // this should be computed instead + + toplevelLayout->setSpacing(0); + toplevelLayout->setContentsMargins(0, 0, 0, 0); + m_listWidget->setSpacing(0); + m_listWidget->setContentsMargins(0, 0, 0, 0); + m_stackWidget->setContentsMargins(0, 0, 0, 0); + setLayout(toplevelLayout); + + connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int))); +} + +void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon &icon, const QString &label) +{ + tab->setContentsMargins(0, 0, 0, 0); + m_stackWidget->insertWidget(index, tab); + QListWidgetItem *item = new QListWidgetItem(icon, label); + item->setToolTip(label); + m_listWidget->insertItem(index, item); +} + +void MyTabbedStackWidget::removeTab(int index) +{ + m_stackWidget->removeWidget(m_stackWidget->widget(index)); + QListWidgetItem *item = m_listWidget->item(index); + m_listWidget->removeItemWidget(item); + delete item; +} + +int MyTabbedStackWidget::currentIndex() const +{ + return m_listWidget->currentRow(); +} + +void MyTabbedStackWidget::setCurrentIndex(int index) +{ + m_listWidget->setCurrentRow(index); +} + +void MyTabbedStackWidget::showWidget(int index) +{ + emit currentAboutToShow(index); + m_stackWidget->setCurrentIndex(index); + emit currentChanged(index); +} + +void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget) +{ + widget->hide(); +} + diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h new file mode 100644 index 000000000..428ed8646 --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -0,0 +1,74 @@ +/** + ****************************************************************************** + * + * @file mytabbedstackwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 MYTABBEDSTACKWIDGET_H +#define MYTABBEDSTACKWIDGET_H + +#include "utils/mylistwidget.h" +#include + +/* + * MyTabbedStackWidget is a MyListWidget combined with a QStackedWidget, + * similar in function to QTabWidget. + */ +class QTCREATOR_UTILS_EXPORT MyTabbedStackWidget : public QWidget +{ + Q_OBJECT + +public: + MyTabbedStackWidget(QWidget *parent = 0, bool isVertical = false, bool iconAbove = true); + + void insertTab(int index, QWidget *tab, const QIcon &icon, const QString &label); + void removeTab(int index); + void setIconSize(int size) { m_listWidget->setIconSize(QSize(size, size)); } + + int currentIndex() const; + + void insertCornerWidget(int index, QWidget *widget); + int cornerWidgetCount() { return m_cornerWidgetCount; } + +signals: + void currentAboutToShow(int index); + void currentChanged(int index); + +public slots: + void setCurrentIndex(int index); + +private slots: + void showWidget(int index); + +private: + MyListWidget *m_listWidget; + QStackedWidget *m_stackWidget; + QWidget *m_selectionWidget; + bool m_vertical; + bool m_iconAbove; + int m_cornerWidgetCount; +}; + +#endif // MYTABBEDSTACKWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp new file mode 100644 index 000000000..2a1a7bee4 --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.cpp @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * + * @file mytabwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 "mytabwidget.h" +#include + +MyTabWidget::MyTabWidget(QWidget *parent) + : QTabWidget(parent) +{ + QTabBar *tabBar = QTabWidget::tabBar(); + connect(tabBar, SIGNAL(tabMoved(int, int)), this, SLOT(myTabMoved(int,int))); +} + +void MyTabWidget::moveTab(int from, int to) +{ + QTabBar *tabBar = QTabWidget::tabBar(); + tabBar->moveTab(from, to); +} + + +void MyTabWidget::myTabMoved(int from, int to) +{ + emit tabMoved(from, to); +} diff --git a/ground/openpilotgcs/src/libs/utils/mytabwidget.h b/ground/openpilotgcs/src/libs/utils/mytabwidget.h new file mode 100644 index 000000000..8f941110b --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/mytabwidget.h @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * + * @file mytabwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. + * @brief + * @see The GNU Public License (GPL) Version 3 + * @defgroup + * @{ + * + *****************************************************************************/ +/* + * 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 MYTABWIDGET_H +#define MYTABWIDGET_H + +#include "utils_global.h" + +#include + +/* + * MyTabWidget is a plain QTabWidget with the addition of the signal + * tabMoved(int, int) which QTabBar has but for some reason is + * not made available from QTabWidget. + */ +class QTCREATOR_UTILS_EXPORT MyTabWidget : public QTabWidget +{ + Q_OBJECT + +public: + MyTabWidget(QWidget *parent = 0); + void moveTab(int from, int to); + +private slots: + void myTabMoved(int from, int to); + +signals: + void tabMoved(int from, int to); +}; + +#endif // MYTABWIDGET_H diff --git a/ground/openpilotgcs/src/libs/utils/utils.pri b/ground/openpilotgcs/src/libs/utils/utils.pri index 5f35399b9..f6e523b74 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pri +++ b/ground/openpilotgcs/src/libs/utils/utils.pri @@ -1,9 +1 @@ -macx { - CONFIG(debug, debug|release):LIBS *= -lUtils_debug - else:LIBS *= -lUtils -} else:win32 { - CONFIG(debug, debug|release):LIBS *= -lUtilsd - else:LIBS *= -lUtils -} else { - LIBS += -l$$qtLibraryTarget(Utils) -} +LIBS *= -l$$qtLibraryName(Utils) diff --git a/ground/openpilotgcs/src/libs/utils/utils.pro b/ground/openpilotgcs/src/libs/utils/utils.pro index 7b923a567..804f7c813 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pro +++ b/ground/openpilotgcs/src/libs/utils/utils.pro @@ -45,7 +45,10 @@ SOURCES += reloadpromptutils.cpp \ coordinateconversions.cpp \ pathutils.cpp \ worldmagmodel.cpp \ - homelocationutil.cpp + homelocationutil.cpp \ + mytabbedstackwidget.cpp \ + mytabwidget.cpp \ + mylistwidget.cpp SOURCES += xmlconfig.cpp win32 { @@ -96,7 +99,10 @@ HEADERS += utils_global.h \ coordinateconversions.h \ pathutils.h \ worldmagmodel.h \ - homelocationutil.h + homelocationutil.h \ + mytabbedstackwidget.h \ + mytabwidget.h \ + mylistwidget.h HEADERS += xmlconfig.h FORMS += filewizardpage.ui \ diff --git a/ground/openpilotgcs/src/openpilotgcslibrary.pri b/ground/openpilotgcs/src/openpilotgcslibrary.pri index 9aa6f00d2..55fe13114 100644 --- a/ground/openpilotgcs/src/openpilotgcslibrary.pri +++ b/ground/openpilotgcs/src/openpilotgcslibrary.pri @@ -8,7 +8,7 @@ DESTDIR = $$GCS_LIBRARY_PATH include(rpath.pri) -TARGET = $$qtLibraryTarget($$TARGET) +TARGET = $$qtLibraryName($$TARGET) contains(QT_CONFIG, reduce_exports):CONFIG += hGCS_symbols diff --git a/ground/openpilotgcs/src/openpilotgcsplugin.pri b/ground/openpilotgcs/src/openpilotgcsplugin.pri index 9668a2e66..d274b71e8 100644 --- a/ground/openpilotgcs/src/openpilotgcsplugin.pri +++ b/ground/openpilotgcs/src/openpilotgcsplugin.pri @@ -23,7 +23,7 @@ copy2build.name = COPY ${QMAKE_FILE_IN} copy2build.CONFIG += no_link QMAKE_EXTRA_COMPILERS += copy2build -TARGET = $$qtLibraryTarget($$TARGET) +TARGET = $$qtLibraryName($$TARGET) macx { QMAKE_LFLAGS_SONAME = -Wl,-install_name,@executable_path/../Plugins/$${PROVIDER}/ diff --git a/ground/openpilotgcs/src/plugins/config/airframe.ui b/ground/openpilotgcs/src/plugins/config/airframe.ui index 4b2b44ce8..fd039a906 100644 --- a/ground/openpilotgcs/src/plugins/config/airframe.ui +++ b/ground/openpilotgcs/src/plugins/config/airframe.ui @@ -23,7 +23,7 @@ Mixer Settings - + 5 @@ -1138,7 +1138,7 @@ Typical value is 50% for + or X configuration on quads. - Throtle Curve 1 + Curve 1 @@ -1208,7 +1208,7 @@ Typical value is 50% for + or X configuration on quads. - Throtle Curve 2 + Curve 2 @@ -1731,75 +1731,6 @@ Typical value is 50% for + or X configuration on quads. - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 0 - 0 - - - - - 32 - 32 - - - - - - - - :/core/images/helpicon.svg:/core/images/helpicon.svg - - - - 32 - 32 - - - - true - - - - - - - Send to board, but don't save permanently (flash or SD). - - - Apply - - - - - - - Applies and Saves all settings to flash or SD depending on board. - - - Save - - - - - @@ -1808,7 +1739,7 @@ Typical value is 50% for + or X configuration on quads. - + @@ -2082,14 +2013,14 @@ p, li { white-space: pre-wrap; } <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> <tr> <td style="border: none;"> <p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD IS DANGEROUS</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!</p> -<p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.</p></td></tr></table></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Lucida Grande'; font-size:13pt;">Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!</span></p> +<p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Lucida Grande'; font-size:13pt;">Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.</span></p></td></tr></table></body></html> @@ -2106,49 +2037,81 @@ p, li { white-space: pre-wrap; } - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Send to board, but don't save permanently (flash or SD). - - - Apply - - - - - - - Applies and Saves all settings to flash or SD depending on board. - - - Save - - - - - + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 32 + 32 + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 32 + 32 + + + + true + + + + + + + Send to board, but don't save permanently (flash or SD). + + + Apply + + + + + + + Applies and Saves all settings to flash or SD depending on board. + + + Save + + + + + 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..a4a8586e5 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui @@ -0,0 +1,283 @@ + + + CC_HW_Widget + + + + 0 + 0 + 517 + 487 + + + + Form + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + + + :/configgadget/images/coptercontrol.svg + + + true + + + + + + + + + + + + + MainPort + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + + + + + FlexiPort + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Receiver type + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + + + + Telemetry speed: + + + + + + + Select the speed here. + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + 75 + true + + + + + + + Qt::AutoText + + + true + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 75 + true + + + + Changes on this page only take effect after board reset or power cycle + + + true + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 32 + 32 + + + + + 32 + 32 + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 32 + 32 + + + + true + + + + + + + Send to OpenPilot but don't write in SD. +Beware of not locking yourself out! + + + true + + + + + + Apply + + + false + + + + + + + Applies and Saves all settings to SD. +Beware of not locking yourself out! + + + false + + + + + + Save + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 4918feb53..79ddea046 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -24,7 +24,8 @@ HEADERS += configplugin.h \ configoutputwidget.h \ configtaskwidget.h \ configairframewidget.h \ - configtelemetrywidget.h \ + config_pro_hw_widget.h \ + config_cc_hw_widget.h \ configahrswidget.h \ configccattitudewidget.h \ mixercurvewidget.h \ @@ -34,7 +35,9 @@ HEADERS += configplugin.h \ configstabilizationwidget.h \ assertions.h \ calibration.h \ - defaultattitudewidget.h + defaultattitudewidget.h \ + smartsavebutton.h \ + defaulthwsettingswidget.h SOURCES += configplugin.cpp \ configgadgetconfiguration.cpp \ @@ -47,7 +50,8 @@ SOURCES += configplugin.cpp \ configinputwidget.cpp \ configoutputwidget.cpp \ configairframewidget.cpp \ - configtelemetrywidget.cpp \ + config_pro_hw_widget.cpp \ + config_cc_hw_widget.cpp \ configahrswidget.cpp \ configccattitudewidget.cpp \ mixercurvewidget.cpp \ @@ -59,17 +63,21 @@ SOURCES += configplugin.cpp \ legacy-calibration.cpp \ gyro-calibration.cpp \ alignment-calibration.cpp \ - defaultattitudewidget.cpp + defaultattitudewidget.cpp \ + smartsavebutton.cpp \ + defaulthwsettingswidget.cpp FORMS += \ airframe.ui \ - telemetry.ui \ + cc_hw_settings.ui \ + pro_hw_settings.ui \ ahrs.ui \ ccpm.ui \ stabilization.ui \ input.ui \ output.ui \ ccattitude.ui \ - defaultattitude.ui + defaultattitude.ui \ + defaulthwsettings.ui RESOURCES += configgadget.qrc 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..e5adf27f1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -0,0 +1,102 @@ +/** + ****************************************************************************** + * + * @file configtelemetrywidget.h + * @author E. Lafargue & 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 +#include +#include +#include +#include +#include +#include +#include + +ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) +{ + m_telemetry = new Ui_CC_HW_Widget(); + m_telemetry->setupUi(this); + setupButtons(m_telemetry->saveTelemetryToRAM,m_telemetry->saveTelemetryToSD); + addUAVObjectToWidgetRelation("TelemetrySettings","Speed",m_telemetry->telemetrySpeed); + addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); + addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); + addUAVObjectToWidgetRelation("ManualControlSettings","InputMode",m_telemetry->receiverType); + connect(m_telemetry->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); + enableControls(false); + populateWidgets(); + refreshWidgetsValues(); +} + +ConfigCCHWWidget::~ConfigCCHWWidget() +{ + // Do nothing +} + +void ConfigCCHWWidget::refreshValues() +{ +} + +void ConfigCCHWWidget::widgetsContentsChanged() +{ + ConfigTaskWidget::widgetsContentsChanged(); + enableControls(false); + if((m_telemetry->cbFlexi->currentText()==m_telemetry->cbTele->currentText()) && m_telemetry->cbTele->currentText()!="Disabled") + { + m_telemetry->problems->setText("Warning: you have configured the MainPort and the FlexiPort for the same function, this is currently not suported"); + } + else if((m_telemetry->cbTele->currentText()=="Spektrum" ||m_telemetry->cbFlexi->currentText()=="Spektrum") && m_telemetry->receiverType->currentText()!="Spektrum") + { + m_telemetry->problems->setText("Warning: you have a port configured as 'Spektrum' however that is not your selected receiver type"); + } + else if(m_telemetry->cbTele->currentText()=="S.Bus" && m_telemetry->receiverType->currentText()!="S.Bus") + { + m_telemetry->problems->setText("Warning: you have a port configured as 'S.Bus' however that is not your selected receiver type"); + } + else if(m_telemetry->cbTele->currentText()!="S.Bus" && m_telemetry->receiverType->currentText()=="S.Bus") + { + m_telemetry->problems->setText("Warning: you have selected 'S.Bus' as your receiver type however you have no port configured for that protocol"); + } + else if((m_telemetry->cbTele->currentText()!="Spektrum" && m_telemetry->cbFlexi->currentText()!="Spektrum") && m_telemetry->receiverType->currentText()=="Spektrum") + { + m_telemetry->problems->setText("Warning: you have selected 'Spektrum' as your receiver type however you have no port configured for that protocol"); + } + else + { + m_telemetry->problems->setText(""); + enableControls(true); + } +} + +void ConfigCCHWWidget::openHelp() +{ + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/CopterControl+HW+Settings", 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..f815786fb --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * + * @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 "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(); + +private: + Ui_CC_HW_Widget *m_telemetry; +}; + +#endif // CONFIGCCHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp new file mode 100644 index 000000000..b3f562acd --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file configtelemetrywidget.h + * @author E. Lafargue & 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_pro_hw_widget.h" + +#include +#include +#include +#include +#include +#include + + +ConfigProHWWidget::ConfigProHWWidget(QWidget *parent) : ConfigTaskWidget(parent) +{ + m_telemetry = new Ui_PRO_HW_Widget(); + m_telemetry->setupUi(this); + + setupButtons(m_telemetry->saveTelemetryToRAM,m_telemetry->saveTelemetryToSD); + addUAVObjectToWidgetRelation("TelemetrySettings","Speed",m_telemetry->telemetrySpeed); + enableControls(false); + populateWidgets(); + refreshWidgetsValues(); +} + +ConfigProHWWidget::~ConfigProHWWidget() +{ + // Do nothing +} + + +/** + Request telemetry settings from the board + */ +void ConfigProHWWidget::refreshValues() +{ +} diff --git a/ground/openpilotgcs/src/plugins/config/configtelemetrywidget.h b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.h similarity index 77% rename from ground/openpilotgcs/src/plugins/config/configtelemetrywidget.h rename to ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.h index f36ba1a4e..fbf35b9bd 100644 --- a/ground/openpilotgcs/src/plugins/config/configtelemetrywidget.h +++ b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.h @@ -24,10 +24,10 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef CONFIGTELEMETRYWIDGET_H -#define CONFIGTELEMETRYWIDGET_H +#ifndef CONFIGPROHWWIDGET_H +#define CONFIGPROHWWIDGET_H -#include "ui_telemetry.h" +#include "ui_pro_hw_settings.h" #include "configtaskwidget.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" @@ -36,23 +36,20 @@ #include -class ConfigTelemetryWidget: public ConfigTaskWidget +class ConfigProHWWidget: public ConfigTaskWidget { Q_OBJECT public: - ConfigTelemetryWidget(QWidget *parent = 0); - ~ConfigTelemetryWidget(); + ConfigProHWWidget(QWidget *parent = 0); + ~ConfigProHWWidget(); private: - Ui_TelemetryWidget *m_telemetry; - void enableControls(bool enable); + Ui_PRO_HW_Widget *m_telemetry; private slots: virtual void refreshValues(); - void sendTelemetryUpdate(); - void saveTelemetryUpdate(); }; -#endif // ConfigTelemetryWidget_H +#endif // CONFIGPROHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp index 0044afa62..dcf743915 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp @@ -91,6 +91,32 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p m_aircraft = new Ui_AircraftWidget(); m_aircraft->setupUi(this); + setupButtons(m_aircraft->saveAircraftToRAM,m_aircraft->saveAircraftToSD); + addWidget(m_aircraft->customMixerTable); + addWidget(m_aircraft->customThrottle2Curve); + addWidget(m_aircraft->customThrottle1Curve); + addWidget(m_aircraft->multiThrottleCurve); + addWidget(m_aircraft->fixedWingThrottle); + addWidget(m_aircraft->fixedWingType); + addWidget(m_aircraft->feedForwardSlider); + addWidget(m_aircraft->accelTime); + addWidget(m_aircraft->decelTime); + addWidget(m_aircraft->maxAccelSlider); + addWidget(m_aircraft->multirotorFrameType); + addWidget(m_aircraft->multiMotor1); + addWidget(m_aircraft->multiMotor2); + addWidget(m_aircraft->multiMotor3); + addWidget(m_aircraft->multiMotor4); + addWidget(m_aircraft->multiMotor5); + addWidget(m_aircraft->multiMotor6); + addWidget(m_aircraft->multiMotor7); + addWidget(m_aircraft->multiMotor8); + addWidget(m_aircraft->triYawChannel); + addUAVObject("SystemSettings"); + addUAVObject("MixerSettings"); + addUAVObject("ActuatorSettings"); + + ffTuningInProgress = false; ffTuningPhase = false; @@ -164,11 +190,6 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p m_aircraft->customMixerTable->setItemDelegateForRow(i, sbd); } - connect(m_aircraft->saveAircraftToSD, SIGNAL(clicked()), this, SLOT(saveAircraftUpdate())); - connect(m_aircraft->saveAircraftToRAM, SIGNAL(clicked()), this, SLOT(sendAircraftUpdate())); - - connect(m_aircraft->ffSave, SIGNAL(clicked()), this, SLOT(saveAircraftUpdate())); - connect(m_aircraft->ffApply, SIGNAL(clicked()), this, SLOT(sendAircraftUpdate())); connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupAirframeUI(QString))); connect(m_aircraft->multirotorFrameType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupAirframeUI(QString))); connect(m_aircraft->aircraftType, SIGNAL(currentIndexChanged(int)), this, SLOT(switchAirframeType(int))); @@ -191,17 +212,8 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p connect(m_aircraft->ffTestBox3, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); enableControls(false); - refreshValues(); - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect())); + refreshWidgetsValues(); - // Register for ManualControlSettings changes: - obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorSettings"))); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); // Connect the help button connect(m_aircraft->airframeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); @@ -213,18 +225,6 @@ ConfigAirframeWidget::~ConfigAirframeWidget() // Do nothing } -/** - Enable or disable controls depending on whether we're ronnected or not - */ -void ConfigAirframeWidget::enableControls(bool enable) -{ - //m_aircraft->saveAircraftToRAM->setEnabled(enable); - m_aircraft->saveAircraftToSD->setEnabled(enable); - //m_aircraft->ffApply->setEnabled(enable); - m_aircraft->ffSave->setEnabled(enable); -} - - /** Slot for switching the airframe type. We do it explicitely rather than a signal in the UI, because we want to force a fitInView of the quad shapes. @@ -460,7 +460,7 @@ void ConfigAirframeWidget::updateCustomThrottle2CurveValue(QList list, d /** Refreshes the current value of the SystemSettings which holds the aircraft type */ -void ConfigAirframeWidget::refreshValues() +void ConfigAirframeWidget::refreshWidgetsValues() { // Get the Airframe type from the system settings: UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); @@ -665,10 +665,13 @@ void ConfigAirframeWidget::refreshValues() val = floor(-field->getDouble(i)/1.27); m_aircraft->mrYawMixLevel->setValue(val); eng = m_aircraft->multiMotor2->currentIndex()-1; - field = obj->getField(mixerVectors.at(eng)); - i = field->getElementNames().indexOf("Roll"); - val = floor(1-field->getDouble(i)/1.27); - m_aircraft->mrRollMixLevel->setValue(val); + if(eng>-1) + { + field = obj->getField(mixerVectors.at(eng)); + i = field->getElementNames().indexOf("Roll"); + val = floor(1-field->getDouble(i)/1.27); + m_aircraft->mrRollMixLevel->setValue(val); + } } } else if (frameType == "HexaX") { // Motors 1/2/3 4/5/6 are: NE / E / SE / SW / W / NW @@ -1817,8 +1820,9 @@ void ConfigAirframeWidget::updateCustomAirframeUI() we call additional methods for specific frames, so that we do not have a code that is too heavy. */ -void ConfigAirframeWidget::sendAircraftUpdate() +void ConfigAirframeWidget::updateObjectsFromWidgets() { + qDebug()<<"updateObjectsFromWidgets"; QString airframeType = "Custom"; if (m_aircraft->aircraftType->currentText() == "Fixed Wing") { // Save the curve (common to all Fixed wing frames) @@ -2123,31 +2127,9 @@ void ConfigAirframeWidget::sendAircraftUpdate() } } - - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorSettings"))); - obj->updated(); - obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - obj->updated(); - obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); + UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); UAVObjectField* field = obj->getField(QString("AirframeType")); field->setValue(airframeType); - obj->updated(); -} - -/** - Send airframe type to the board and request saving to SD card - */ -void ConfigAirframeWidget::saveAircraftUpdate() -{ - // Send update so that the latest value is saved - sendAircraftUpdate(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); - Q_ASSERT(obj); - saveObjectToSD(obj); - obj = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - saveObjectToSD(obj); - obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorSettings"))); - saveObjectToSD(obj); } diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.h b/ground/openpilotgcs/src/plugins/config/configairframewidget.h index 5d9bfecc8..234a114c1 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.h @@ -58,7 +58,6 @@ private: void updateCustomAirframeUI(); bool setupMixer(double mixerFactors[8][3]); void setupMotors(QList motorList); - virtual void enableControls(bool enable); void resetField(UAVObjectField * field); void resetMixer (MixerCurveWidget *mixer, int numElements, double maxvalue); @@ -74,9 +73,9 @@ private: UAVObject::Metadata accInitialData; private slots: - virtual void refreshValues(); - void sendAircraftUpdate(); - void saveAircraftUpdate(); + virtual void refreshWidgetsValues(); + void updateObjectsFromWidgets(); + // void saveAircraftUpdate(); void setupAirframeUI(QString type); void toggleAileron2(int index); void toggleElevator2(int index); diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp index 4e9e2810a..e53cbb70c 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp @@ -37,6 +37,9 @@ #include #include +#include "mixersettings.h" +#include "systemsettings.h" + #define Pi 3.14159265358979323846 @@ -1179,7 +1182,7 @@ void ConfigccpmWidget::requestccpmUpdate() { #define MaxAngleError 2 int MixerDataFromHeli[8][5]; - QString MixerOutputType[8]; + quint8 MixerOutputType[8]; int EngineChannel,TailRotorChannel,ServoChannels[4],ServoAngles[4],SortAngles[4],CalcAngles[4],ServoCurve2[4]; int NumServos=0; double Collective=0.0; @@ -1191,39 +1194,45 @@ void ConfigccpmWidget::requestccpmUpdate() if (updatingToHardware)return; updatingFromHardware=TRUE; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); int i,j; - UAVObjectField *field; - UAVDataObject* obj; - obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); - field = obj->getField(QString("GUIConfigData")); - GUIConfigData.UAVObject[0]=field->getValue(0).toUInt(); - GUIConfigData.UAVObject[1]=field->getValue(1).toUInt(); - UpdatCCPMUIFromOptions(); + SystemSettings * systemSettings = SystemSettings::GetInstance(getObjectManager()); + SystemSettings::DataFields systemSettingsData = systemSettings->getData(); + Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == + (sizeof(GUIConfigData.UAVObject) / sizeof(GUIConfigData.UAVObject[0]))); + + for(i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) + GUIConfigData.UAVObject[i]=systemSettingsData.GUIConfigData[i]; + + UpdatCCPMUIFromOptions(); + + // Get existing mixer settings + MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); - obj = dynamic_cast(objManager->getObject(QString("MixerSettings"))); - Q_ASSERT(obj); - //go through the user data and update the mixer matrix - for (i=0;i<8;i++) + for (j=0;j<5;j++) { - field = obj->getField(tr( "Mixer%1Vector" ).arg(i+1)); - //config the vector - for (j=0;j<5;j++) - { - MixerDataFromHeli[i][j] = field->getValue(j).toInt(); - //field->setValue(m_ccpm->ccpmAdvancedSettingsTable->item(i,j+1)->text().toInt(),j); - } - } - for (i=0;i<8;i++) - { - field = obj->getField(tr( "Mixer%1Type" ).arg(i+1)); - MixerOutputType[i] = field->getValue().toString(); + MixerDataFromHeli[0][j] = mixerSettingsData.Mixer1Vector[j]; + MixerDataFromHeli[1][j] = mixerSettingsData.Mixer2Vector[j]; + MixerDataFromHeli[2][j] = mixerSettingsData.Mixer3Vector[j]; + MixerDataFromHeli[3][j] = mixerSettingsData.Mixer4Vector[j]; + MixerDataFromHeli[4][j] = mixerSettingsData.Mixer5Vector[j]; + MixerDataFromHeli[5][j] = mixerSettingsData.Mixer6Vector[j]; + MixerDataFromHeli[6][j] = mixerSettingsData.Mixer7Vector[j]; + MixerDataFromHeli[7][j] = mixerSettingsData.Mixer8Vector[j]; } + MixerOutputType[0] = mixerSettingsData.Mixer1Type; + MixerOutputType[1] = mixerSettingsData.Mixer2Type; + MixerOutputType[2] = mixerSettingsData.Mixer3Type; + MixerOutputType[3] = mixerSettingsData.Mixer4Type; + MixerOutputType[4] = mixerSettingsData.Mixer5Type; + MixerOutputType[5] = mixerSettingsData.Mixer6Type; + MixerOutputType[6] = mixerSettingsData.Mixer7Type; + MixerOutputType[7] = mixerSettingsData.Mixer8Type; + EngineChannel =-1; TailRotorChannel =-1; for (j=0;j<5;j++) @@ -1239,7 +1248,7 @@ void ConfigccpmWidget::requestccpmUpdate() for (i=0;i<8;i++) { //check if this is the engine... Throttle only - if ((MixerOutputType[i].compare("Motor")==0)&& + if ((MixerOutputType[i] == MixerSettings::MIXER1TYPE_MOTOR)&& (MixerDataFromHeli[i][0]>0)&&//ThrottleCurve1 (MixerDataFromHeli[i][1]==0)&&//ThrottleCurve2 (MixerDataFromHeli[i][2]==0)&&//Roll @@ -1251,7 +1260,7 @@ void ConfigccpmWidget::requestccpmUpdate() } //check if this is the tail rotor... REVO and YAW - if ((MixerOutputType[i].compare("Servo")==0)&& + if ((MixerOutputType[i] == MixerSettings::MIXER1TYPE_SERVO)&& //(MixerDataFromHeli[i][0]!=0)&&//ThrottleCurve1 (MixerDataFromHeli[i][1]==0)&&//ThrottleCurve2 (MixerDataFromHeli[i][2]==0)&&//Roll @@ -1264,7 +1273,7 @@ void ConfigccpmWidget::requestccpmUpdate() m_ccpm->ccpmREVOspinBox->setValue((MixerDataFromHeli[i][0]*100)/127); } //check if this is a swashplate servo... Throttle is zero - if ((MixerOutputType[i].compare("Servo")==0)&& + if ((MixerOutputType[i] == MixerSettings::MIXER1TYPE_SERVO)&& (MixerDataFromHeli[i][0]==0)&&//ThrottleCurve1 //(MixerDataFromHeli[i][1]==0)&&//ThrottleCurve2 //(MixerDataFromHeli[i][2]==0)&&//Roll @@ -1275,193 +1284,23 @@ void ConfigccpmWidget::requestccpmUpdate() ServoCurve2[NumServos]=MixerDataFromHeli[i][1];//record the ThrottleCurve2 contribution to this servo ServoAngles[NumServos]=NumServos*45;//make this 0 for the final version - //if (NumServos==0)m_ccpm->ccpmServoWChannel->setCurrentIndex(i); - //if (NumServos==1)m_ccpm->ccpmServoXChannel->setCurrentIndex(i); - //if (NumServos==2)m_ccpm->ccpmServoYChannel->setCurrentIndex(i); - //if (NumServos==3)m_ccpm->ccpmServoZChannel->setCurrentIndex(i); NumServos++; } } - - - - //just call it user angles for now.... - /* - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("Custom - User Angles")); - - if (NumServos>1) - { - if((ServoCurve2[0]==0)&&(ServoCurve2[1]==0)&&(ServoCurve2[2]==0)&&(ServoCurve2[3]==0)) - { - //fixed pitch heli - isCCPM=0; - m_ccpm->ccpmCollectiveSlider->setValue(0); - Collective = 0.0; - } - if(ServoCurve2[0]==ServoCurve2[1]) - { - if ((NumServos<3)||(ServoCurve2[1]==ServoCurve2[2])) - { - if ((NumServos<4)||(ServoCurve2[2]==ServoCurve2[3])) - {//all the servos have the same ThrottleCurve2 setting so this must be a CCPM config - isCCPM=1; - Collective = ((double)ServoCurve2[0]*100.00)/127.00; - m_ccpm->ccpmCollectiveSlider->setValue((int)Collective); - m_ccpm->ccpmCollectivespinBox->setValue((int)Collective); - - } - } - } - } - else - {//must be a custom config... "Custom - Advanced Settings" - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("Custom - Advanced Settings")); - } - - HeadRotation=0; - - HeadRotation=m_ccpm->ccpmSingleServo->currentIndex(); - //calculate the angles - for(j=0;jccpmSingleServo->setCurrentIndex(HeadRotation); - - //calculate the un rotated angles - for(j=0;j CalcAngles[SortAngles[j+1]]) - {//swap the sorted angles - temp = SortAngles[j]; - SortAngles[j]=SortAngles[j+1]; - SortAngles[j+1]=temp; - } - - } - - //m_ccpm->ccpmAngleW->setValue(ServoAngles[SortAngles[0]]); - //m_ccpm->ccpmAngleX->setValue(ServoAngles[SortAngles[1]]); - //m_ccpm->ccpmAngleY->setValue(ServoAngles[SortAngles[2]]); - //m_ccpm->ccpmAngleZ->setValue(ServoAngles[SortAngles[3]]); - - //m_ccpm->ccpmServoWChannel->setCurrentIndex(ServoChannels[SortAngles[0]]); - //m_ccpm->ccpmServoXChannel->setCurrentIndex(ServoChannels[SortAngles[1]]); - //m_ccpm->ccpmServoYChannel->setCurrentIndex(ServoChannels[SortAngles[2]]); - //m_ccpm->ccpmServoZChannel->setCurrentIndex(ServoChannels[SortAngles[3]]); - m_ccpm->ccpmServoWChannel->setCurrentIndex(ServoChannels[0]); - m_ccpm->ccpmServoXChannel->setCurrentIndex(ServoChannels[1]); - m_ccpm->ccpmServoYChannel->setCurrentIndex(ServoChannels[2]); - m_ccpm->ccpmServoZChannel->setCurrentIndex(ServoChannels[3]); - - - //Types << "CCPM 2 Servo 90º" << "CCPM 3 Servo 120º" << "CCPM 3 Servo 140º" << "FP 2 Servo 90º" << "Custom - User Angles" << "Custom - Advanced Settings" ; - - - //check this against known combinations - if (NumServos==2) - { - if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 2 Servo 90º")); - UpdateType(); - } - else - { - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 2 Servo 90º")); - UpdateType(); - } - - } - } - if (NumServos==3) - { - if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 3 Servo 120º")); - UpdateType(); - - } - else - { - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 3 Servo 120º")); - UpdateType(); - } - - } - else if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 3 Servo 140º")); - UpdateType(); - } - else - { - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 3 Servo 140º")); - UpdateType(); - } - - } - - } - if (NumServos==4) - { - - } -*/ - - - //get the settings for the curve from the mixer settings - field = obj->getField(QString("ThrottleCurve1")); for (i=0;i<5;i++) { - m_ccpm->CurveSettings->item(i, 0)->setText(QString().sprintf("%.3f",field->getValue(i).toDouble())); - //m_ccpm->CurveSettings->item(i, 0)->setText(field->getValue(i).toString()); + m_ccpm->CurveSettings->item(i, 0)->setText(QString().sprintf("%.3f", + mixerSettingsData.ThrottleCurve1[i])); + m_ccpm->CurveSettings->item(i, 1)->setText(QString().sprintf("%.3f", + mixerSettingsData.ThrottleCurve2[i])); } - field = obj->getField(QString("ThrottleCurve2")); - for (i=0;i<5;i++) - { - m_ccpm->CurveSettings->item(i, 1)->setText(QString().sprintf("%.3f",field->getValue(i).toDouble())); - //m_ccpm->CurveSettings->item(i, 1)->setText(field->getValue(i).toString()); - } - - - updatingFromHardware=FALSE; UpdatCCPMUIFromOptions(); ccpmSwashplateUpdate(); - } @@ -1499,7 +1338,7 @@ void ConfigccpmWidget::sendccpmUpdate() //clear the output types for (i=0;i<8;i++) { - field = obj->getField(tr( "Mixer%1Type" ).arg( i+1 )); + field = obj->getField( QString( "Mixer%1Type" ).arg( i+1 )); //clear the mixer type field->setValue("Disabled"); } @@ -1520,7 +1359,7 @@ void ConfigccpmWidget::sendccpmUpdate() if (MixerChannelData[i]<8) { //select the correct mixer for this config element - field = obj->getField(tr( "Mixer%1Type" ).arg( MixerChannelData[i]+1 )); + field = obj->getField(QString( "Mixer%1Type" ).arg( MixerChannelData[i]+1 )); //set the mixer type if (i==0) { @@ -1532,7 +1371,7 @@ void ConfigccpmWidget::sendccpmUpdate() } //select the correct mixer for this config element - field = obj->getField(tr( "Mixer%1Vector" ).arg( MixerChannelData[i]+1 )); + field = obj->getField(QString( "Mixer%1Vector" ).arg( MixerChannelData[i]+1 )); //config the vector for (j=0;j<5;j++) { diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index 30f9431e3..5f982053b 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -1,18 +1,19 @@ - - images/help2.png - images/XBee.svg - images/Airframe.png - images/Servo.png - images/ahrs-calib.svg - images/AHRS-v1.3.png - images/paper-plane.svg - images/curve-bg.svg - images/quad-shapes.svg - images/ccpm_setup.svg - images/PipXtreme.png - images/gyroscope.svg - images/Transmitter.png - images/help.png - + + images/help2.png + images/Airframe.png + images/Servo.png + images/ahrs-calib.svg + images/AHRS-v1.3.png + images/paper-plane.svg + images/curve-bg.svg + images/quad-shapes.svg + images/ccpm_setup.svg + images/PipXtreme.png + images/Transmitter.png + images/help.png + images/coptercontrol.svg + images/hw_config.png + images/gyroscope.png + diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index f19b414c4..39acb5adf 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -33,9 +33,10 @@ #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" -#include "configtelemetrywidget.h" +#include "config_pro_hw_widget.h" +#include "config_cc_hw_widget.h" #include "defaultattitudewidget.h" - +#include "defaulthwsettingswidget.h" #include "uavobjectutilmanager.h" #include @@ -51,34 +52,35 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); - ftw = new FancyTabWidget(this, true); - + ftw = new MyTabbedStackWidget(this, true, true); ftw->setIconSize(64); QVBoxLayout *layout = new QVBoxLayout; + layout->setContentsMargins(0, 0, 0, 0); layout->addWidget(ftw); setLayout(layout); // ********************* QWidget *qwd; + qwd = new DefaultHwSettingsWidget(this); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + qwd = new ConfigAirframeWidget(this); - ftw->insertTab(0, qwd, QIcon(":/configgadget/images/Airframe.png"), QString("Aircraft")); + ftw->insertTab(ConfigGadgetWidget::aircraft, qwd, QIcon(":/configgadget/images/Airframe.png"), QString("Aircraft")); qwd = new ConfigInputWidget(this); - ftw->insertTab(1, qwd, QIcon(":/configgadget/images/Transmitter.png"), QString("Input")); + ftw->insertTab(ConfigGadgetWidget::input, qwd, QIcon(":/configgadget/images/Transmitter.png"), QString("Input")); qwd = new ConfigOutputWidget(this); - ftw->insertTab(2, qwd, QIcon(":/configgadget/images/Servo.png"), QString("Output")); + ftw->insertTab(ConfigGadgetWidget::output, qwd, QIcon(":/configgadget/images/Servo.png"), QString("Output")); qwd = new DefaultAttitudeWidget(this); - ftw->insertTab(3, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->insertTab(ConfigGadgetWidget::ins, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); qwd = new ConfigStabilizationWidget(this); - ftw->insertTab(4, qwd, QIcon(":/configgadget/images/gyroscope.svg"), QString("Stabilization")); + ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization")); - qwd = new ConfigTelemetryWidget(this); - ftw->insertTab(5, qwd, QIcon(":/configgadget/images/XBee.svg"), QString("Telemetry")); // qwd = new ConfigPipXtremeWidget(this); @@ -128,16 +130,24 @@ void ConfigGadgetWidget::onAutopilotConnect() { if ((board & 0xff00) == 1024) { // CopterControl family // Delete the INS panel, replace with CC Panel: - ftw->setCurrentIndex(0); - ftw->removeTab(3); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); + ftw->removeTab(ConfigGadgetWidget::ins); QWidget *qwd = new ConfigCCAttitudeWidget(this); - ftw->insertTab(3, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("Attitude")); + ftw->insertTab(ConfigGadgetWidget::ins, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("Attitude")); + ftw->removeTab(ConfigGadgetWidget::hardware); + qwd = new ConfigCCHWWidget(this); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); } else if ((board & 0xff00) == 256 ) { // Mainboard family - ftw->setCurrentIndex(0); - ftw->removeTab(3); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); + ftw->removeTab(ConfigGadgetWidget::ins); QWidget *qwd = new ConfigAHRSWidget(this); - ftw->insertTab(3, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->insertTab(ConfigGadgetWidget::ins, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->removeTab(ConfigGadgetWidget::hardware); + qwd = new ConfigProHWWidget(this); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); } } emit autopilotConnected(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 43db58429..df2f553a3 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -38,7 +38,8 @@ #include #include "utils/pathutils.h" -#include "fancytabwidget.h" +//#include "fancytabwidget.h" +#include "utils/mytabbedstackwidget.h" class ConfigGadgetWidget: public QWidget @@ -49,6 +50,7 @@ class ConfigGadgetWidget: public QWidget public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); + enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization}; public slots: void onAutopilotConnect(); @@ -60,8 +62,7 @@ signals: protected: void resizeEvent(QResizeEvent * event); - FancyTabWidget *ftw; - + MyTabbedStackWidget *ftw; }; #endif // CONFIGGADGETWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 5fb4b5b73..83e1a02ac 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -110,10 +110,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) // Get the receiver types supported by OpenPilot and fill the corresponding // dropdown menu: obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - QString fieldName = QString("InputMode"); - UAVObjectField *field = obj->getField(fieldName); - m_config->receiverType->addItems(field->getOptions()); - + UAVObjectField * field; // Fill in the dropdown menus for the channel RC Input assignement. QStringList channelsList; channelsList << "None"; @@ -263,7 +260,7 @@ void ConfigInputWidget::refreshValues() // Update receiver type field = obj->getField(QString("InputMode")); - m_config->receiverType->setCurrentIndex(m_config->receiverType->findText(field->getValue().toString())); + m_config->receiverType->setText(field->getValue().toString()); // Reset all channel assignement dropdowns: foreach (QComboBox *combo, inChannelAssign) { @@ -331,11 +328,6 @@ void ConfigInputWidget::sendRCInputUpdate() for (int i = 0; i < 8; i++) field->setValue(inSliders[i]->value(), i); - // Set RC Receiver type: - fieldName = QString("InputMode"); - field = obj->getField(fieldName); - field->setValue(m_config->receiverType->currentText()); - // Set Roll/Pitch/Yaw/Etc assignement: // Rule: if two channels have the same setting (which is wrong!) the higher channel // will get the setting. @@ -706,7 +698,7 @@ void ConfigInputWidget::receiverHelp() unassigned.append("FlightMode"); } if(unassigned.length()>0) - m_config->lblMissingInputs->setText(QString("Channels left to assign:")+unassigned); + m_config->lblMissingInputs->setText(QString("Channels left to assign: ")+unassigned); else m_config->lblMissingInputs->setText(""); } diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 83ac65523..5cd0cfa04 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -230,6 +230,8 @@ void ConfigStabilizationWidget::refreshValues() m_stabilization->maximumPitch->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH]); m_stabilization->maximumYaw->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW]); + m_stabilization->lowThrottleZeroIntegral->setChecked( + stabData.LowThrottleZeroIntegral == StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE); } @@ -277,6 +279,10 @@ void ConfigStabilizationWidget::sendStabilizationUpdate() stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH] = m_stabilization->maximumPitch->value(); stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW] = m_stabilization->maximumYaw->value(); + stabData.LowThrottleZeroIntegral = m_stabilization->lowThrottleZeroIntegral->isChecked() ? + StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE : + StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_FALSE; + stabSettings->setData(stabData); // this is atomic } diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index bb598cba0..8fd35c596 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -28,13 +28,72 @@ #include -ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent) +ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent),smartsave(NULL),dirty(false) { + pm = ExtensionSystem::PluginManager::instance(); + objManager = pm->getObject(); + connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); + connect(parent, SIGNAL(autopilotDisconnected()),this, SLOT(onAutopilotDisconnect())); } +void ConfigTaskWidget::addWidget(QWidget * widget) +{ + addUAVObjectToWidgetRelation("","",widget); +} +void ConfigTaskWidget::addUAVObject(QString objectName) +{ + addUAVObjectToWidgetRelation(objectName,"",NULL); +} +void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget * widget) +{ + UAVObject *obj=NULL; + UAVObjectField *_field=NULL; + if(!object.isEmpty()) + obj = objManager->getObject(QString(object)); + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); + //smartsave->addObject(obj); + if(!field.isEmpty() && obj) + _field = obj->getField(QString(field)); + objectToWidget * ow=new objectToWidget(); + ow->field=_field; + ow->object=obj; + ow->widget=widget; + objOfInterest.append(ow); + if(obj) + smartsave->addObject(obj); + if(widget==NULL) + { + // do nothing + } + else if(QComboBox * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(currentIndexChanged(int)),this,SLOT(widgetsContentsChanged())); + } + else if(QSlider * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(sliderMoved(int)),this,SLOT(widgetsContentsChanged())); + } + else if(MixerCurveWidget * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(curveUpdated(QList,double)),this,SLOT(widgetsContentsChanged())); + } + else if(QTableWidget * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(cellChanged(int,int)),this,SLOT(widgetsContentsChanged())); + } + else if(QSpinBox * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(valueChanged(int)),this,SLOT(widgetsContentsChanged())); + } + else if(QDoubleSpinBox * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(valueChanged(double)),this,SLOT(widgetsContentsChanged())); + } +} + ConfigTaskWidget::~ConfigTaskWidget() { - // Do nothing + delete smartsave; } void ConfigTaskWidget::saveObjectToSD(UAVObject *obj) @@ -73,9 +132,125 @@ void ConfigTaskWidget::onAutopilotDisconnect() void ConfigTaskWidget::onAutopilotConnect() { enableControls(true); - refreshValues(); + refreshWidgetsValues(); } +void ConfigTaskWidget::populateWidgets() +{ + foreach(objectToWidget * ow,objOfInterest) + { + if(ow->object==NULL || ow->field==NULL) + { + // do nothing + } + else if(QComboBox * cb=qobject_cast(ow->widget)) + { + cb->addItems(ow->field->getOptions()); + cb->setCurrentIndex(cb->findText(ow->field->getValue().toString())); + } + else if(QLabel * cb=qobject_cast(ow->widget)) + { + cb->setText(ow->field->getValue().toString()); + } + } + dirty=false; +} + +void ConfigTaskWidget::refreshWidgetsValues() +{ + foreach(objectToWidget * ow,objOfInterest) + { + if(ow->object==NULL || ow->field==NULL) + { + //do nothing + } + else if(QComboBox * cb=qobject_cast(ow->widget)) + { + cb->setCurrentIndex(cb->findText(ow->field->getValue().toString())); + } + else if(QLabel * cb=qobject_cast(ow->widget)) + { + cb->setText(ow->field->getValue().toString()); + } + } +} + +void ConfigTaskWidget::updateObjectsFromWidgets() +{ + foreach(objectToWidget * ow,objOfInterest) + { + if(ow->object==NULL || ow->field==NULL) + { + //do nothing + } + else if(QComboBox * cb=qobject_cast(ow->widget)) + { + ow->field->setValue(cb->currentText()); + } + else if(QLabel * cb=qobject_cast(ow->widget)) + { + ow->field->setValue(cb->text()); + } + } +} + +void ConfigTaskWidget::setupButtons(QPushButton *update, QPushButton *save) +{ + smartsave=new smartSaveButton(update,save); + connect(smartsave, SIGNAL(preProcessOperations()), this, SLOT(updateObjectsFromWidgets())); + connect(smartsave,SIGNAL(saveSuccessfull()),this,SLOT(clearDirty())); + connect(smartsave,SIGNAL(beginOp()),this,SLOT(disableObjUpdates())); + connect(smartsave,SIGNAL(endOp()),this,SLOT(enableObjUpdates())); +} + +void ConfigTaskWidget::enableControls(bool enable) +{ + if(smartsave) + smartsave->enableControls(enable); +} + +void ConfigTaskWidget::widgetsContentsChanged() +{ + dirty=true; +} + +void ConfigTaskWidget::clearDirty() +{ + dirty=false; +} + +bool ConfigTaskWidget::isDirty() +{ + return dirty; +} + +void ConfigTaskWidget::refreshValues() +{ +} + +void ConfigTaskWidget::disableObjUpdates() +{ + foreach(objectToWidget * obj,objOfInterest) + { + if(obj->object) + disconnect(obj->object, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); + } +} + +void ConfigTaskWidget::enableObjUpdates() +{ + foreach(objectToWidget * obj,objOfInterest) + { + if(obj->object) + connect(obj->object, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); + } +} + + + + + + /** diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h index 6ffe15b70..bd293ce5f 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h @@ -35,28 +35,56 @@ #include #include #include - - +#include +#include "smartsavebutton.h" +#include "mixercurvewidget.h" +#include +#include +#include class ConfigTaskWidget: public QWidget { Q_OBJECT public: + struct objectToWidget + { + UAVObject * object; + UAVObjectField * field; + QWidget * widget; + }; + ConfigTaskWidget(QWidget *parent = 0); ~ConfigTaskWidget(); void saveObjectToSD(UAVObject *obj); UAVObjectManager* getObjectManager(); static double listMean(QList list); - + void addUAVObject(QString objectName); + void addWidget(QWidget * widget); + void addUAVObjectToWidgetRelation(QString object,QString field,QWidget * widget); + void setupButtons(QPushButton * update,QPushButton * save); + bool isDirty(); public slots: void onAutopilotDisconnect(); void onAutopilotConnect(); private slots: - virtual void refreshValues() = 0; - + virtual void refreshValues(); + virtual void updateObjectsFromWidgets(); private: - virtual void enableControls(bool enable) = 0; + QList objOfInterest; + ExtensionSystem::PluginManager *pm; + UAVObjectManager *objManager; + smartSaveButton *smartsave; + bool dirty; +protected slots: + virtual void disableObjUpdates(); + virtual void enableObjUpdates(); + virtual void clearDirty(); + virtual void widgetsContentsChanged(); + virtual void populateWidgets(); + virtual void refreshWidgetsValues(); +protected: + virtual void enableControls(bool enable); }; diff --git a/ground/openpilotgcs/src/plugins/config/configtelemetrywidget.cpp b/ground/openpilotgcs/src/plugins/config/configtelemetrywidget.cpp deleted file mode 100644 index 2b1a8e873..000000000 --- a/ground/openpilotgcs/src/plugins/config/configtelemetrywidget.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/** - ****************************************************************************** - * - * @file configtelemetrywidget.h - * @author E. Lafargue & 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 "configtelemetrywidget.h" - -#include -#include -#include -#include -#include -#include - - -ConfigTelemetryWidget::ConfigTelemetryWidget(QWidget *parent) : ConfigTaskWidget(parent) -{ - m_telemetry = new Ui_TelemetryWidget(); - m_telemetry->setupUi(this); - - // Now connect the widget to the ManualControlCommand / Channel UAVObject - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - - UAVObject *obj = objManager->getObject(QString("TelemetrySettings")); - UAVObjectField *field = obj->getField(QString("Speed")); - m_telemetry->telemetrySpeed->addItems(field->getOptions()); - - connect(m_telemetry->saveTelemetryToSD, SIGNAL(clicked()), this, SLOT(saveTelemetryUpdate())); - connect(m_telemetry->saveTelemetryToRAM, SIGNAL(clicked()), this, SLOT(sendTelemetryUpdate())); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); - - enableControls(false); - refreshValues(); - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()),this, SLOT(onAutopilotDisconnect())); -} - -ConfigTelemetryWidget::~ConfigTelemetryWidget() -{ - // Do nothing -} - - -/******************************* - * Telemetry Settings - *****************************/ - -void ConfigTelemetryWidget::enableControls(bool enable) -{ - m_telemetry->saveTelemetryToSD->setEnabled(enable); - //m_telemetry->saveTelemetryToRAM->setEnabled(enable); -} - -/** - Request telemetry settings from the board - */ -void ConfigTelemetryWidget::refreshValues() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("TelemetrySettings"))); - Q_ASSERT(obj); - UAVObjectField *field = obj->getField(QString("Speed")); - m_telemetry->telemetrySpeed->setCurrentIndex(m_telemetry->telemetrySpeed->findText(field->getValue().toString())); -} - -/** - Send telemetry settings to the board - */ -void ConfigTelemetryWidget::sendTelemetryUpdate() -{ - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("TelemetrySettings"))); - Q_ASSERT(obj); - UAVObjectField* field = obj->getField(QString("Speed")); - field->setValue(m_telemetry->telemetrySpeed->currentText()); - obj->updated(); -} - -/** - Send telemetry settings to the board and request saving to SD card - */ -void ConfigTelemetryWidget::saveTelemetryUpdate() -{ - // Send update so that the latest value is saved - sendTelemetryUpdate(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("TelemetrySettings"))); - Q_ASSERT(obj); - saveObjectToSD(obj); -} diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettings.ui b/ground/openpilotgcs/src/plugins/config/defaulthwsettings.ui new file mode 100644 index 000000000..d1ef7d913 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettings.ui @@ -0,0 +1,77 @@ + + + defaulthwsettings + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Hardware Configuration</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-weight:600;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This panel will be updated to provide the relevant controls to let you configure your hardware once telemetry is connected and running.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p></body></html> + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp new file mode 100644 index 000000000..69eef9117 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.cpp @@ -0,0 +1,44 @@ +/** + ****************************************************************************** + * + * @file DefaultHwSettingsWidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Placeholder for attitude panel until board is connected. + *****************************************************************************/ +/* + * 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 "defaulthwsettingswidget.h" +#include "ui_defaultattitude.h" +#include +#include +#include + +DefaultHwSettingsWidget::DefaultHwSettingsWidget(QWidget *parent) : + QWidget(parent), + ui(new Ui_defaulthwsettings) +{ + ui->setupUi(this); +} + +DefaultHwSettingsWidget::~DefaultHwSettingsWidget() +{ + delete ui; +} + diff --git a/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h new file mode 100644 index 000000000..e94435f8d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/defaulthwsettingswidget.h @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * + * @file defaultccattitudewidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Placeholder for attitude settings widget until board connected. + *****************************************************************************/ +/* + * 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 DEFAULTHWSETTINGSt_H +#define DEFAULTHWSETTINGSt_H + +#include "ui_defaulthwsettings.h" +#include "configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include +#include + +class Ui_Widget; + +class DefaultHwSettingsWidget : public QWidget +{ + Q_OBJECT + +public: + explicit DefaultHwSettingsWidget(QWidget *parent = 0); + ~DefaultHwSettingsWidget(); + +private slots: + +private: + Ui_defaulthwsettings *ui; +}; + +#endif // DEFAULTHWSETTINGSt_H 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/config/images/gyroscope.png b/ground/openpilotgcs/src/plugins/config/images/gyroscope.png new file mode 100644 index 000000000..b6277f5cc Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/gyroscope.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/hw_config.png b/ground/openpilotgcs/src/plugins/config/images/hw_config.png new file mode 100644 index 000000000..ff69b90f3 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/hw_config.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/hw_config.svg b/ground/openpilotgcs/src/plugins/config/images/hw_config.svg new file mode 100644 index 000000000..a40c3c3f3 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/images/hw_config.svg @@ -0,0 +1,88 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 320aa2db8..e1eb0b435 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -6,8 +6,8 @@ 0 0 - 655 - 554 + 626 + 532 @@ -24,34 +24,12 @@ RC Input - - - - - 50 - 0 - - - - - 16777215 - 16777215 - - - - You must restart your board after changing receiver type. Also your hardware setings must support it. - - - true - - - - + - 50 - false + 75 + true @@ -62,17 +40,7 @@ - - - - Select the receiver type here: -- PWM is the most usual type -- PPM is connected to input XXX -- Spektrum is used with Spektrum 'satellite' receivers - - - - + @@ -84,7 +52,6 @@ - 11 75 true @@ -102,14 +69,17 @@ - + Rev. + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + - + @@ -122,39 +92,7 @@ - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -173,7 +111,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -188,7 +126,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -200,42 +138,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -254,7 +160,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -269,7 +175,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -281,42 +187,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -335,7 +209,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -350,7 +224,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -362,42 +236,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -416,7 +258,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -431,7 +273,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -443,42 +285,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -497,7 +307,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -512,7 +322,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -524,42 +334,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -578,7 +356,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -593,7 +371,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -605,42 +383,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -659,7 +405,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -674,7 +420,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -686,42 +432,10 @@ reversal capabilities). - + - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + true @@ -740,7 +454,7 @@ p, li { white-space: pre-wrap; } - + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> @@ -755,7 +469,7 @@ p, li { white-space: pre-wrap; } - + Check this to reverse the channel. @@ -767,7 +481,34 @@ reversal capabilities). - + + + + + 75 + true + + + + BEWARE: make sure your engines are not connected when running calibration! + + + + + + + + + 75 + true + + + + + + + + @@ -787,30 +528,266 @@ Neutral should be put at the bottom of the slider for the throttle. - - - - - 75 - true - - + + - - - - - 75 - true - + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - BEWARE: make sure your engines are not connected when running calibration! - + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> + + + 1000 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 + + + + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> + + + 1500 @@ -1192,19 +1169,6 @@ if you have not done so already. - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -1287,7 +1251,6 @@ Applies and Saves all settings to SD ch5Assign ch6Assign ch7Assign - receiverType fmsSlider fmsModePos3 fmsSsPos3Roll diff --git a/ground/openpilotgcs/src/plugins/config/telemetry.ui b/ground/openpilotgcs/src/plugins/config/pro_hw_settings.ui similarity index 98% rename from ground/openpilotgcs/src/plugins/config/telemetry.ui rename to ground/openpilotgcs/src/plugins/config/pro_hw_settings.ui index 0db9c9299..6e6751b87 100644 --- a/ground/openpilotgcs/src/plugins/config/telemetry.ui +++ b/ground/openpilotgcs/src/plugins/config/pro_hw_settings.ui @@ -1,7 +1,7 @@ - TelemetryWidget - + PRO_HW_Widget + 0 diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp new file mode 100644 index 000000000..ed5abf2c7 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp @@ -0,0 +1,122 @@ +#include "smartsavebutton.h" + +smartSaveButton::smartSaveButton(QPushButton * update, QPushButton * save):bupdate(update),bsave(save) +{ + connect(bsave,SIGNAL(clicked()),this,SLOT(processClick())); + connect(bupdate,SIGNAL(clicked()),this,SLOT(processClick())); + +} +void smartSaveButton::processClick() +{ + emit beginOp(); + bool save=false; + QPushButton *button=bupdate; + if(sender()==bsave) + { + save=true; + button=bsave; + } + emit preProcessOperations(); + button->setEnabled(false); + button->setIcon(QIcon(":/uploader/images/system-run.svg")); + QTimer timer; + timer.setSingleShot(true); + bool error=false; + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectUtilManager* utilMngr = pm->getObject(); + foreach(UAVObject * obj,objects) + { + up_result=false; + current_object=obj; + for(int i=0;i<3;++i) + { + + connect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); + connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + obj->updated(); + timer.start(1000); + loop.exec(); + timer.stop(); + disconnect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); + disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + if(up_result) + break; + } + if(up_result==false) + { + error=true; + continue; + } + sv_result=false; + current_objectID=obj->getObjID(); + if(save) + { + for(int i=0;i<3;++i) + { + connect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); + connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + utilMngr->saveObjectToSD(obj); + timer.start(1000); + loop.exec(); + timer.stop(); + disconnect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); + disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + if(sv_result) + break; + } + if(sv_result==false) + { + error=true; + } + } + } + button->setEnabled(true); + if(!error) + { + button->setIcon(QIcon(":/uploader/images/dialog-apply.svg")); + emit saveSuccessfull(); + } + else + { + button->setIcon(QIcon(":/uploader/images/process-stop.svg")); + } + emit endOp(); +} + +void smartSaveButton::setObjects(QList list) +{ + objects=list; +} + +void smartSaveButton::addObject(UAVObject * obj) +{ + objects.append(obj); +} + +void smartSaveButton::clearObjects() +{ + objects.clear(); +} +void smartSaveButton::transaction_finished(UAVObject* obj, bool result) +{ + if(current_object==obj) + { + up_result=result; + loop.quit(); + } +} + +void smartSaveButton::saving_finished(int id, bool result) +{ + if(id==current_objectID) + { + sv_result=result; + loop.quit(); + } +} + +void smartSaveButton::enableControls(bool value) +{ + bupdate->setEnabled(value); + bsave->setEnabled(value); +} diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.h b/ground/openpilotgcs/src/plugins/config/smartsavebutton.h new file mode 100644 index 000000000..d0238d034 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.h @@ -0,0 +1,49 @@ +#ifndef SMARTSAVEBUTTON_H +#define SMARTSAVEBUTTON_H + +#include "uavtalk/telemetrymanager.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include +#include +#include "uavobjectutilmanager.h" +#include +#include +class smartSaveButton:public QObject +{ +public: + Q_OBJECT +public: + smartSaveButton(QPushButton * update,QPushButton * save); + void setObjects(QList); + void addObject(UAVObject *); + void clearObjects(); +signals: + void preProcessOperations(); + void saveSuccessfull(); + void beginOp(); + void endOp(); +private slots: + void processClick(); + void transaction_finished(UAVObject* obj, bool result); + void saving_finished(int,bool); + +private: + QPushButton *bupdate; + QPushButton *bsave; + quint32 current_objectID; + UAVObject * current_object; + bool up_result; + bool sv_result; + QEventLoop loop; + QList objects; +protected: +public slots: + void enableControls(bool value); + +}; + + +#endif // SMARTSAVEBUTTON_H diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 70f5f3cb3..96c414055 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -629,6 +629,13 @@ When you change one, the other is updated. + + + + Zero the integral when throttle is low + + + diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 176263d04..f63c6e7da 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -32,36 +32,33 @@ #include #include -#include - -#include "fancytabwidget.h" -#include "fancyactionbar.h" #include "qextserialport/src/qextserialenumerator.h" #include "qextserialport/src/qextserialport.h" #include #include #include #include -#include + namespace Core { -ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, Internal::FancyTabWidget *modeStack) : +ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) : QWidget(mainWindow), // Pip m_availableDevList(0), m_connectBtn(0), - m_ioDev(NULL),m_mainWindow(mainWindow) + m_ioDev(NULL), + m_mainWindow(mainWindow) { // Q_UNUSED(mainWindow); - QVBoxLayout *top = new QVBoxLayout; +/* QVBoxLayout *top = new QVBoxLayout; top->setSpacing(0); - top->setMargin(0); + top->setMargin(0);*/ QHBoxLayout *layout = new QHBoxLayout; - layout->setSpacing(0); - layout->setContentsMargins(5,0,5,0); - layout->addWidget(new QLabel("Connections: ")); + layout->setSpacing(5); + layout->setContentsMargins(5,5,5,5); + layout->addWidget(new QLabel(tr("Connections:"))); m_availableDevList = new QComboBox; //m_availableDevList->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); @@ -70,17 +67,18 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, Internal: m_availableDevList->setContextMenuPolicy(Qt::CustomContextMenu); layout->addWidget(m_availableDevList); - m_connectBtn = new QPushButton("Connect"); + m_connectBtn = new QPushButton(tr("Connect")); m_connectBtn->setEnabled(false); layout->addWidget(m_connectBtn); - Utils::StyledBar *bar = new Utils::StyledBar; +/* Utils::StyledBar *bar = new Utils::StyledBar; bar->setLayout(layout); - top->addWidget(bar); - setLayout(top); + top->addWidget(bar);*/ + setLayout(layout); - modeStack->insertCornerWidget(modeStack->cornerWidgetCount()-1, this); + // modeStack->insertCornerWidget(modeStack->cornerWidgetCount()-1, this); + modeStack->setCornerWidget(this, Qt::TopRightCorner); QObject::connect(m_connectBtn, SIGNAL(pressed()), this, SLOT(onConnectPressed())); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 085111700..beb809915 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -40,6 +40,10 @@ #include "core_global.h" +QT_BEGIN_NAMESPACE +class QTabWidget; +QT_END_NAMESPACE + namespace Core { class IConnection; @@ -65,7 +69,7 @@ class CORE_EXPORT ConnectionManager : public QWidget Q_OBJECT public: - ConnectionManager(Internal::MainWindow *mainWindow, Internal::FancyTabWidget *modeStack); + ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack); virtual ~ConnectionManager(); void init(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp index dd6c3d0bc..b45582628 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.cpp @@ -187,9 +187,9 @@ void CoreImpl::openFiles(const QStringList &arguments) //m_mainwindow->openFiles(arguments); } -void CoreImpl::readMainSettings(QSettings* qs) +void CoreImpl::readMainSettings(QSettings* qs, bool workspaceDiffOnly) { - m_mainwindow->readSettings(qs); + m_mainwindow->readSettings(qs, workspaceDiffOnly); } void CoreImpl::saveMainSettings(QSettings* qs) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h index eaebdc3d0..cd0dfd928 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreimpl.h @@ -64,7 +64,7 @@ public: QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const; SettingsDatabase *settingsDatabase() const; - void readMainSettings(QSettings* qs); + void readMainSettings(QSettings* qs, bool workspaceDiffOnly); void saveMainSettings(QSettings* qs); void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0 ); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pri b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pri index fc5fc7aae..26143cbc1 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pri +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pri @@ -1,2 +1,2 @@ include(coreplugin_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(Core) +LIBS *= -l$$qtLibraryName(Core) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro index 70ac509e8..8c73b00ef 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro @@ -24,10 +24,8 @@ SOURCES += mainwindow.cpp \ uniqueidmanager.cpp \ messagemanager.cpp \ messageoutputwindow.cpp \ - viewmanager.cpp \ versiondialog.cpp \ iuavgadget.cpp \ - uavgadgetmode.cpp \ uavgadgetmanager/uavgadgetmanager.cpp \ uavgadgetmanager/uavgadgetview.cpp \ uavgadgetmanager/splitterorview.cpp \ @@ -73,9 +71,7 @@ HEADERS += mainwindow.h \ uniqueidmanager.h \ messagemanager.h \ messageoutputwindow.h \ - viewmanager.h \ iuavgadget.h \ - uavgadgetmode.h \ iuavgadgetfactory.h \ uavgadgetmanager/uavgadgetmanager.h \ uavgadgetmanager/uavgadgetview.h \ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp index e3e2602e8..f0bc7bf4f 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp @@ -44,9 +44,10 @@ using namespace Utils; using namespace Core::Internal; GeneralSettings::GeneralSettings(): - m_dialog(0), m_saveSettingsOnExit(true), - m_autoConnect(true),m_autoSelect(true) + m_dialog(0), + m_autoConnect(true), + m_autoSelect(true) { } @@ -119,24 +120,9 @@ QWidget *GeneralSettings::createPage(QWidget *parent) m_page->checkAutoConnect->setChecked(m_autoConnect); m_page->checkAutoSelect->setChecked(m_autoSelect); m_page->colorButton->setColor(StyleHelper::baseColor()); -#ifdef Q_OS_UNIX - m_page->terminalEdit->setText(ConsoleProcess::terminalEmulator(Core::ICore::instance()->settings())); -#else - m_page->terminalLabel->hide(); - m_page->terminalEdit->hide(); - m_page->resetTerminalButton->hide(); -#endif connect(m_page->resetButton, SIGNAL(clicked()), this, SLOT(resetInterfaceColor())); - connect(m_page->resetEditorButton, SIGNAL(clicked()), - this, SLOT(resetExternalEditor())); - connect(m_page->helpExternalEditorButton, SIGNAL(clicked()), - this, SLOT(showHelpForExternalEditor())); -#ifdef Q_OS_UNIX - connect(m_page->resetTerminalButton, SIGNAL(clicked()), - this, SLOT(resetTerminal())); -#endif return w; } @@ -151,11 +137,6 @@ void GeneralSettings::apply() m_saveSettingsOnExit = m_page->checkBoxSaveOnExit->isChecked(); m_autoConnect = m_page->checkAutoConnect->isChecked(); m_autoSelect = m_page->checkAutoSelect->isChecked(); -#ifdef Q_OS_UNIX - ConsoleProcess::setTerminalEmulator(Core::ICore::instance()->settings(), - m_page->terminalEdit->text()); -#endif - } void GeneralSettings::finish() @@ -171,7 +152,6 @@ void GeneralSettings::readSettings(QSettings* qs) m_autoConnect = qs->value(QLatin1String("AutoConnect"),m_autoConnect).toBool(); m_autoSelect = qs->value(QLatin1String("AutoSelect"),m_autoSelect).toBool(); qs->endGroup(); - } void GeneralSettings::saveSettings(QSettings* qs) @@ -194,17 +174,6 @@ void GeneralSettings::resetInterfaceColor() m_page->colorButton->setColor(0x666666); } -void GeneralSettings::resetExternalEditor() -{ -} - -#ifdef Q_OS_UNIX -void GeneralSettings::resetTerminal() -{ - m_page->terminalEdit->setText(ConsoleProcess::defaultTerminalEmulator() + QLatin1String(" -e")); -} -#endif - void GeneralSettings::showHelpForExternalEditor() { if (m_dialog) { @@ -238,10 +207,11 @@ QString GeneralSettings::language() const void GeneralSettings::setLanguage(const QString &locale) { - if (m_language != locale) - { + if (m_language != locale) { + if (!locale.isEmpty()) { QMessageBox::information((QWidget*)Core::ICore::instance()->mainWindow(), tr("Restart required"), tr("The language change will take effect after a restart of the OpenPilot GCS.")); + } m_language = locale; } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h index ad4790578..f70d7c9a9 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h @@ -61,15 +61,13 @@ public: void readSettings(QSettings* qs); void saveSettings(QSettings* qs); +signals: + private slots: void resetInterfaceColor(); void resetLanguage(); - void resetExternalEditor(); void showHelpForExternalEditor(); void slotAutoConnect(int); -#ifdef Q_OS_UNIX - void resetTerminal(); -#endif private: void fillLanguageBox() const; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.ui b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.ui index 752ba2bfb..1be8020ce 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.ui +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.ui @@ -17,33 +17,6 @@ General settings - - - - Terminal: - - - - - - - External editor: - - - - - - - When files are externally modified: - - - true - - - - - - @@ -51,44 +24,6 @@ - - - - Reset to default - - - R - - - - :/core/images/reset.png:/core/images/reset.png - - - - - - - - - - Reset to default - - - R - - - - :/core/images/reset.png:/core/images/reset.png - - - - - - - ? - - - @@ -144,53 +79,12 @@ - + 0 - - - - - 0 - 0 - - - - 0 - - - - Always ask - - - - - Reload all modified files - - - - - Ignore modifications - - - - - - - - Qt::Horizontal - - - - 132 - 20 - - - - @@ -221,7 +115,7 @@ - + @@ -231,27 +125,27 @@ - + - Save configuration settings on on exit + Save configuration settings on exit: true - + - Automatically connect an OpenPilot USB device + Automatically connect an OpenPilot USB device: true - + @@ -261,17 +155,17 @@ - + - Automatically select an OpenPilot USB device + Automatically select an OpenPilot USB device: true - + false diff --git a/ground/openpilotgcs/src/plugins/coreplugin/icore.h b/ground/openpilotgcs/src/plugins/coreplugin/icore.h index 6e8110ca1..f06479883 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/icore.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/icore.h @@ -88,7 +88,7 @@ public: virtual QSettings *settings(QSettings::Scope scope = QSettings::UserScope) const = 0; virtual SettingsDatabase *settingsDatabase() const = 0; - virtual void readMainSettings(QSettings* qs) = 0; + virtual void readMainSettings(QSettings* qs, bool workspaceDiffOnly = false) = 0; virtual void saveMainSettings(QSettings* qs) = 0; virtual void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; virtual void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0) = 0; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/imode.h b/ground/openpilotgcs/src/plugins/coreplugin/imode.h index 50d31eba7..f548acd0a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/imode.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/imode.h @@ -49,6 +49,7 @@ public: virtual QString name() const = 0; virtual QIcon icon() const = 0; virtual int priority() const = 0; + virtual void setPriority(int priority) = 0; virtual const char *uniqueModeName() const = 0; }; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index 25170092e..a2b8137f4 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -33,7 +33,7 @@ #include "connectionmanager.h" #include "coreimpl.h" #include "coreconstants.h" -#include "fancytabwidget.h" +#include "utils/mytabwidget.h" #include "generalsettings.h" #include "messagemanager.h" #include "modemanager.h" @@ -43,7 +43,6 @@ #include "qxtlogger.h" #include "qxtbasicstdloggerengine.h" #include "shortcutsettings.h" -#include "uavgadgetmode.h" #include "uavgadgetmanager.h" #include "uavgadgetinstancemanager.h" #include "workspacesettings.h" @@ -60,7 +59,6 @@ #include "uniqueidmanager.h" #include "variablemanager.h" #include "versiondialog.h" -#include "viewmanager.h" #include #include @@ -123,7 +121,6 @@ MainWindow::MainWindow() : m_actionManager(new ActionManagerPrivate(this)), m_variableManager(new VariableManager(this)), m_threadManager(new ThreadManager(this)), - m_viewManager(0), m_modeManager(0), m_connectionManager(0), m_mimeDatabase(new MimeDatabase), @@ -178,17 +175,25 @@ MainWindow::MainWindow() : registerDefaultContainers(); registerDefaultActions(); - m_modeStack = new FancyTabWidget(this); + m_modeStack = new MyTabWidget(this); + m_modeStack->setIconSize(QSize(24,24)); + m_modeStack->setTabPosition(QTabWidget::South); + m_modeStack->setMovable(false); +#ifndef Q_WS_MAC + m_modeStack->setDocumentMode(true); +#endif m_modeManager = new ModeManager(this, m_modeStack); m_connectionManager = new ConnectionManager(this, m_modeStack); - m_viewManager = new ViewManager(this); m_messageManager = new MessageManager; setCentralWidget(m_modeStack); connect(QApplication::instance(), SIGNAL(focusChanged(QWidget*,QWidget*)), this, SLOT(updateFocusWidget(QWidget*,QWidget*))); + connect(m_workspaceSettings, SIGNAL(tabBarSettingsApplied(QTabWidget::TabPosition,bool)), + this, SLOT(applyTabBarSettings(QTabWidget::TabPosition,bool))); + connect(m_modeManager, SIGNAL(newModeOrder(QVector)), m_workspaceSettings, SLOT(newModeOrder(QVector))); // setUnifiedTitleAndToolBarOnMac(true); #ifdef Q_OS_UNIX @@ -218,8 +223,8 @@ MainWindow::~MainWindow() foreach (QString engine, qxtLog->allLoggerEngines()) qxtLog->removeLoggerEngine(engine); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (m_uavGadgetModes.count() > 0) { - foreach (UAVGadgetMode *mode, m_uavGadgetModes) + if (m_uavGadgetManagers.count() > 0) { + foreach (UAVGadgetManager *mode, m_uavGadgetManagers) { pm->removeObject(mode); delete mode; @@ -242,9 +247,6 @@ MainWindow::~MainWindow() delete m_uniqueIDManager; m_uniqueIDManager = 0; - delete m_viewManager; - m_viewManager = 0; - pm->removeObject(m_coreImpl); delete m_coreImpl; m_coreImpl = 0; @@ -264,7 +266,6 @@ bool MainWindow::init(QString *errorMessage) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); pm->addObject(m_coreImpl); - m_viewManager->init(); m_modeManager->init(); m_connectionManager->init(); @@ -393,7 +394,7 @@ IContext *MainWindow::currentContextObject() const QStatusBar *MainWindow::statusBar() const { - return m_modeStack->statusBar(); + return new QStatusBar();// m_modeStack->statusBar(); } void MainWindow::registerDefaultContainers() @@ -669,6 +670,60 @@ void MainWindow::registerDefaultActions() connect(m_toggleFullScreenAction, SIGNAL(triggered(bool)), this, SLOT(setFullScreen(bool))); #endif + /* + * UavGadgetManager Actions + */ + const QList uavGadgetManagerContext = + QList() << CoreImpl::instance()->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + //Window menu separators + QAction *tmpaction1 = new QAction(this); + tmpaction1->setSeparator(true); + cmd = am->registerAction(tmpaction1, QLatin1String("OpenPilot.Window.Sep.Split"), uavGadgetManagerContext); + mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); + + m_showToolbarsAction = new QAction(tr("Edit Gadgets Mode"), this); + m_showToolbarsAction->setCheckable(true); + cmd = am->registerAction(m_showToolbarsAction, Constants::HIDE_TOOLBARS, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+F10"))); + mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); + + //Window menu separators + QAction *tmpaction2 = new QAction(this); + tmpaction2->setSeparator(true); + cmd = am->registerAction(tmpaction2, QLatin1String("OpenPilot.Window.Sep.Split2"), uavGadgetManagerContext); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + +#ifdef Q_WS_MAC + QString prefix = tr("Meta+Shift"); +#else + QString prefix = tr("Ctrl+Shift"); +#endif + + m_splitAction = new QAction(tr("Split"), this); + cmd = am->registerAction(m_splitAction, Constants::SPLIT, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("%1+Down").arg(prefix))); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + + m_splitSideBySideAction = new QAction(tr("Split Side by Side"), this); + cmd = am->registerAction(m_splitSideBySideAction, Constants::SPLIT_SIDE_BY_SIDE, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("%1+Right").arg(prefix))); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + + m_removeCurrentSplitAction = new QAction(tr("Close Current View"), this); + cmd = am->registerAction(m_removeCurrentSplitAction, Constants::REMOVE_CURRENT_SPLIT, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("%1+C").arg(prefix))); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + + m_removeAllSplitsAction = new QAction(tr("Close All Other Views"), this); + cmd = am->registerAction(m_removeAllSplitsAction, Constants::REMOVE_ALL_SPLITS, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("%1+A").arg(prefix))); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + + m_gotoOtherSplitAction = new QAction(tr("Goto Next View"), this); + cmd = am->registerAction(m_gotoOtherSplitAction, Constants::GOTO_OTHER_SPLIT, uavGadgetManagerContext); + cmd->setDefaultKeySequence(QKeySequence(tr("%1+N").arg(prefix))); + mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); + //Help Action tmpaction = new QAction(QIcon(Constants::ICON_HELP), tr("&Help..."), this); cmd = am->registerAction(tmpaction, Constants::G_HELP_HELP, m_globalContext); @@ -807,6 +862,12 @@ void MainWindow::openFileWith() } +void MainWindow::applyTabBarSettings(QTabWidget::TabPosition pos, bool movable) { + if (m_modeStack->tabPosition() != pos) + m_modeStack->setTabPosition(pos); + m_modeStack->setMovable(movable); +} + ActionManager *MainWindow::actionManager() const { return m_actionManager; @@ -845,12 +906,6 @@ ConnectionManager *MainWindow::connectionManager() const return m_connectionManager; } -void MainWindow::addUAVGadgetManager(UAVGadgetManager *manager) -{ - if (!m_uavGadgetManagers.contains(manager)) - m_uavGadgetManagers.append(manager); -} - QList MainWindow::uavGadgetManagers() const { return m_uavGadgetManagers; @@ -986,40 +1041,83 @@ void MainWindow::shutdown() uavGadgetInstanceManager()->removeAllGadgets(); } -void MainWindow::createWorkspaces() { +/* Enable/disable menus for uavgadgets */ +void MainWindow::showUavGadgetMenus(bool show, bool hasSplitter) +{ + m_showToolbarsAction->setChecked(show); + m_splitAction->setEnabled(show); + m_splitSideBySideAction->setEnabled(show); + m_removeCurrentSplitAction->setEnabled(show && hasSplitter); + m_removeAllSplitsAction->setEnabled(show && hasSplitter); + m_gotoOtherSplitAction->setEnabled(show && hasSplitter); +} + +inline int takeLeastPriorityUavGadgetManager(const QList m_uavGadgetManagers) { + int index = 0; + int prio = m_uavGadgetManagers.at(0)->priority(); + for (int i = 0; i < m_uavGadgetManagers.count(); i++) { + int prio2 = m_uavGadgetManagers.at(i)->priority(); + if (prio2 < prio) { + prio = prio2; + index = i; + } + } + return index; +} + +void MainWindow::createWorkspaces(QSettings* qs, bool diffOnly) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVGadgetMode *uavGadgetMode; Core::UAVGadgetManager *uavGadgetManager; - while (!m_uavGadgetModes.isEmpty()){ - pm->removeObject(m_uavGadgetModes.takeFirst()); - } - while (!m_uavGadgetManagers.isEmpty()){ - Core::UAVGadgetManager* uavGadgetManager = m_uavGadgetManagers.takeLast(); - uavGadgetManager->removeAllSplits(); - // TODO Fixthis: This only happens, when settings are reloaded. - // then the old GadgetManagers should be deleted, but if - // I delete them the GCS segfaults. - //delete uavGadgetManager; - } - m_uavGadgetManagers.clear(); - m_uavGadgetModes.clear(); - for (int i = 0; i < m_workspaceSettings->numberOfWorkspaces(); ++i) { + // If diffOnly is true, we only add/remove the number of workspaces + // that has changed, + // otherwise a complete reload of workspaces is done + int toRemoveFirst = m_uavGadgetManagers.count(); + int newWorkspacesNo = m_workspaceSettings->numberOfWorkspaces(); + if (diffOnly && m_uavGadgetManagers.count() > newWorkspacesNo) + toRemoveFirst = m_uavGadgetManagers.count() - newWorkspacesNo; + else + toRemoveFirst = 0; + + int removed = 0; + + while (!m_uavGadgetManagers.isEmpty() && (toRemoveFirst > removed)) { + int index = takeLeastPriorityUavGadgetManager(m_uavGadgetManagers); + uavGadgetManager = m_uavGadgetManagers.takeAt(index); + uavGadgetManager->removeAllSplits(); + pm->removeObject(uavGadgetManager); + delete uavGadgetManager; + removed++; + } + + int start = 0; + if (diffOnly) { + start = m_uavGadgetManagers.count(); + } else { + m_uavGadgetManagers.clear(); + } + for (int i = start; i < newWorkspacesNo; ++i) { - uavGadgetManager = new Core::UAVGadgetManager(CoreImpl::instance(), this); - uavGadgetManager->hide(); const QString name = m_workspaceSettings->name(i); const QString iconName = m_workspaceSettings->iconName(i); const QString modeName = m_workspaceSettings->modeName(i); + uavGadgetManager = new Core::UAVGadgetManager(CoreImpl::instance(), name, + QIcon(iconName), 90-i+1, modeName, this); - uavGadgetMode = new UAVGadgetMode(uavGadgetManager, name, - QIcon(iconName), 90-i+1, modeName); - uavGadgetManager->setUAVGadgetMode(uavGadgetMode); - m_uavGadgetModes.append(uavGadgetMode); - pm->addObject(uavGadgetMode); - addUAVGadgetManager(uavGadgetManager); + connect(uavGadgetManager, SIGNAL(showUavGadgetMenus(bool, bool)), this, SLOT(showUavGadgetMenus(bool, bool))); + + connect(m_showToolbarsAction, SIGNAL(triggered(bool)), uavGadgetManager, SLOT(showToolbars(bool))); + connect(m_splitAction, SIGNAL(triggered()), uavGadgetManager, SLOT(split())); + connect(m_splitSideBySideAction, SIGNAL(triggered()), uavGadgetManager, SLOT(splitSideBySide())); + connect(m_removeCurrentSplitAction, SIGNAL(triggered()), uavGadgetManager, SLOT(removeCurrentSplit())); + connect(m_removeAllSplitsAction, SIGNAL(triggered()), uavGadgetManager, SLOT(removeAllSplits())); + connect(m_gotoOtherSplitAction, SIGNAL(triggered()), uavGadgetManager, SLOT(gotoOtherSplit())); + + pm->addObject(uavGadgetManager); + m_uavGadgetManagers.append(uavGadgetManager); + uavGadgetManager->readSettings(qs); } } @@ -1028,13 +1126,19 @@ static const char *geometryKey = "Geometry"; static const char *colorKey = "Color"; static const char *maxKey = "Maximized"; static const char *fullScreenKey = "FullScreen"; +static const char *modePriorities = "ModePriorities"; -void MainWindow::readSettings(QSettings* qs) +void MainWindow::readSettings(QSettings* qs, bool workspaceDiffOnly) { if ( !qs ){ qs = m_settings; } + if (workspaceDiffOnly) { + createWorkspaces(qs, workspaceDiffOnly); + return; + } + m_generalSettings->readSettings(qs); m_actionManager->readSettings(qs); @@ -1056,13 +1160,18 @@ void MainWindow::readSettings(QSettings* qs) m_workspaceSettings->readSettings(qs); - createWorkspaces(); + createWorkspaces(qs); - foreach (UAVGadgetManager *manager, m_uavGadgetManagers) { - manager->readSettings(qs); + // Read tab ordering + qs->beginGroup(QLatin1String(modePriorities)); + QStringList modeNames = qs->childKeys(); + QMap map; + foreach (QString modeName, modeNames) { + map.insert(modeName, qs->value(modeName).toInt()); } + m_modeManager->reorderModes(map); - m_viewManager->readSettings(qs); + qs->endGroup(); } @@ -1092,11 +1201,18 @@ void MainWindow::saveSettings(QSettings* qs) qs->endGroup(); + // Write tab ordering + qs->beginGroup(QLatin1String(modePriorities)); + QVector modes = m_modeManager->modes(); + foreach (IMode *mode, modes) { + qs->setValue(QLatin1String(mode->uniqueModeName()), mode->priority()); + } + qs->endGroup(); + foreach (UAVGadgetManager *manager, m_uavGadgetManagers) { manager->saveSettings(qs); } - m_viewManager->saveSettings(qs); m_actionManager->saveSettings(qs); m_generalSettings->saveSettings(qs); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h index d2f7b1977..7d1c61ebc 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.h @@ -40,6 +40,7 @@ QT_BEGIN_NAMESPACE class QSettings; class QShortcut; class QToolButton; +class MyTabWidget; QT_END_NAMESPACE namespace Core { @@ -73,10 +74,8 @@ class FancyTabWidget; class GeneralSettings; class ShortcutSettings; class WorkspaceSettings; -class ViewManager; class VersionDialog; class AuthorsDialog; -class UAVGadgetMode; class CORE_EXPORT MainWindow : public EventFilteringMainWindow { @@ -94,7 +93,7 @@ public: void addContextObject(IContext *contex); void removeContextObject(IContext *contex); void resetContext(); - void readSettings(QSettings* qs = 0); + void readSettings(QSettings* qs = 0, bool workspaceDiffOnly = false); void saveSettings(QSettings* qs = 0); void readSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); void saveSettings(IConfigurablePlugin* plugin, QSettings* qs = 0); @@ -162,14 +161,14 @@ private slots: void destroyVersionDialog(); void destroyAuthorsDialog(); void modeChanged(Core::IMode *mode); + void showUavGadgetMenus(bool show, bool hasSplitter); + void applyTabBarSettings(QTabWidget::TabPosition pos, bool movable); private: - void addUAVGadgetManager(Core::UAVGadgetManager *manager); void updateContextObject(IContext *context); void registerDefaultContainers(); void registerDefaultActions(); - - void createWorkspaces(); + void createWorkspaces(QSettings* qs, bool diffOnly = false); CoreImpl *m_coreImpl; UniqueIDManager *m_uniqueIDManager; @@ -183,15 +182,12 @@ private: MessageManager *m_messageManager; VariableManager *m_variableManager; ThreadManager *m_threadManager; - ViewManager *m_viewManager; ModeManager *m_modeManager; QList m_uavGadgetManagers; - QList m_uavGadgetModes; UAVGadgetInstanceManager *m_uavGadgetInstanceManager; ConnectionManager *m_connectionManager; MimeDatabase *m_mimeDatabase; - FancyTabWidget *m_modeStack; -// RightPaneWidget *m_rightPaneWidget; + MyTabWidget *m_modeStack; Core::BaseView *m_outputView; VersionDialog *m_versionDialog; AuthorsDialog *m_authorsDialog; @@ -213,6 +209,14 @@ private: QAction *m_exitAction; QAction *m_optionsAction; QAction *m_toggleFullScreenAction; + // UavGadgetManager actions + QAction *m_showToolbarsAction; + QAction *m_splitAction; + QAction *m_splitSideBySideAction; + QAction *m_removeCurrentSplitAction; + QAction *m_removeAllSplitsAction; + QAction *m_gotoOtherSplitAction; + #ifdef Q_WS_MAC QAction *m_minimizeAction; QAction *m_zoomAction; diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp index 3dc36bbe8..ac3c4c224 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp @@ -30,6 +30,7 @@ #include "fancytabwidget.h" #include "fancyactionbar.h" +#include "utils/mytabwidget.h" #include "icore.h" #include "mainwindow.h" @@ -59,18 +60,17 @@ using namespace Core::Internal; ModeManager *ModeManager::m_instance = 0; -ModeManager::ModeManager(Internal::MainWindow *mainWindow, FancyTabWidget *modeStack) : +ModeManager::ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStack) : m_mainWindow(mainWindow), m_modeStack(modeStack), - m_signalMapper(new QSignalMapper(this)) + m_signalMapper(new QSignalMapper(this)), + m_isReprioritizing(false) { m_instance = this; - m_actionBar = new FancyActionBar(modeStack); -// m_modeStack->addCornerWidget(m_actionBar); - - connect(m_modeStack, SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); - connect(m_modeStack, SIGNAL(currentChanged(int)), SLOT(currentTabChanged(int))); +// connect((m_modeStack), SIGNAL(currentAboutToShow(int)), SLOT(currentTabAboutToChange(int))); + connect(m_modeStack, SIGNAL(currentChanged(int)), this, SLOT(currentTabChanged(int))); + connect(m_modeStack, SIGNAL(tabMoved(int,int)), this, SLOT(tabMoved(int,int))); connect(m_signalMapper, SIGNAL(mapped(QString)), this, SLOT(activateMode(QString))); } @@ -87,7 +87,7 @@ void ModeManager::addWidget(QWidget *widget) // We want the actionbar to stay on the bottom // so m_modeStack->cornerWidgetCount() -1 inserts it at the position immediately above // the actionbar - m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); +// m_modeStack->insertCornerWidget(m_modeStack->cornerWidgetCount() -1, widget); } IMode *ModeManager::currentMode() const @@ -150,6 +150,14 @@ void ModeManager::objectAdded(QObject *obj) m_modeShortcuts.insert(index, cmd); connect(cmd, SIGNAL(keySequenceChanged()), this, SLOT(updateModeToolTip())); + + setDefaultKeyshortcuts(); + + m_signalMapper->setMapping(shortcut, mode->uniqueModeName()); + connect(shortcut, SIGNAL(activated()), m_signalMapper, SLOT(map())); +} + +void ModeManager::setDefaultKeyshortcuts() { for (int i = 0; i < m_modeShortcuts.size(); ++i) { Command *currentCmd = m_modeShortcuts.at(i); bool currentlyHasDefaultSequence = (currentCmd->keySequence() @@ -162,9 +170,6 @@ void ModeManager::objectAdded(QObject *obj) if (currentlyHasDefaultSequence) currentCmd->setKeySequence(currentCmd->defaultKeySequence()); } - - m_signalMapper->setMapping(shortcut, mode->uniqueModeName()); - connect(shortcut, SIGNAL(activated()), m_signalMapper, SLOT(map())); } void ModeManager::updateModeToolTip() @@ -182,7 +187,8 @@ void ModeManager::updateModeNameIcon(IMode *mode, const QIcon &icon, const QStri int index = indexOf(mode->uniqueModeName()); if (index < 0) return; - m_modeStack->updateTabNameIcon(index, icon, label); + m_modeStack->setTabIcon(index, icon); + m_modeStack->setTabText(index, label); } void ModeManager::aboutToRemoveObject(QObject *obj) @@ -194,7 +200,9 @@ void ModeManager::aboutToRemoveObject(QObject *obj) const int index = m_modes.indexOf(mode); m_modes.remove(index); m_modeShortcuts.remove(index); + disconnect(m_modeStack, SIGNAL(currentChanged(int)), this, SLOT(currentTabChanged(int))); m_modeStack->removeTab(index); + connect(m_modeStack, SIGNAL(currentChanged(int)), this, SLOT(currentTabChanged(int))); m_mainWindow->removeContextObject(mode); } @@ -209,7 +217,7 @@ void ModeManager::addAction(Command *command, int priority, QMenu *menu) if (p > priority) ++index; - m_actionBar->insertAction(index, command->action(), menu); +// m_actionBar->insertAction(index, command->action(), menu); } void ModeManager::currentTabAboutToChange(int index) @@ -223,6 +231,7 @@ void ModeManager::currentTabAboutToChange(int index) void ModeManager::currentTabChanged(int index) { +// qDebug() << "Current tab changed " << index; // Tab index changes to -1 when there is no tab left. if (index >= 0) { IMode *mode = m_modes.at(index); @@ -242,6 +251,53 @@ void ModeManager::currentTabChanged(int index) } } +void ModeManager::tabMoved(int from, int to) +{ + IMode *mode = m_modes.at(from); + m_modes.remove(from); + m_modes.insert(to, mode); + Command *cmd = m_modeShortcuts.at(from); + m_modeShortcuts.remove(from); + m_modeShortcuts.insert(to, cmd); + setDefaultKeyshortcuts(); + // Reprioritize, high priority means show to the left + if (!m_isReprioritizing) { + for (int i = 0; i < m_modes.count(); ++i) { + m_modes.at(i)->setPriority(100-i); + } + emit newModeOrder(m_modes); + } +} + +void ModeManager::reorderModes(QMap priorities) +{ + foreach (IMode *mode, m_modes) + mode->setPriority(priorities.value(QString(QLatin1String(mode->uniqueModeName())), mode->priority())); + + m_isReprioritizing = true; + IMode *current = currentMode(); + // Bubble sort + bool swapped = false; + do { + swapped = false; + for (int i = 0; i < m_modes.count()-1; ++i) { + IMode *mode1 = m_modes.at(i); + IMode *mode2 = m_modes.at(i+1); +// qDebug() << "Comparing " << i << " to " << i+1 << " p1 " << mode1->priority() << " p2 " << mode2->priority(); + if (mode2->priority() > mode1->priority()) { + m_modeStack->moveTab(i, i+1); +// qDebug() << "Tab moved from " << i << " to " << i+1; + swapped = true; + } + } + } while (swapped); + m_isReprioritizing = false; + m_modeStack->setCurrentIndex(0); + activateMode(current->uniqueModeName()); + emit newModeOrder(m_modes); +} + + void ModeManager::setFocusToCurrentMode() { IMode *mode = currentMode(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h index 0dbae6a01..687ec0a51 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h @@ -40,6 +40,7 @@ QT_BEGIN_NAMESPACE class QSignalMapper; class QMenu; class QIcon; +class MyTabWidget; QT_END_NAMESPACE namespace Core { @@ -58,7 +59,7 @@ class CORE_EXPORT ModeManager : public QObject Q_OBJECT public: - ModeManager(Internal::MainWindow *mainWindow, Internal::FancyTabWidget *modeStack); + ModeManager(Internal::MainWindow *mainWindow, MyTabWidget *modeStack); void init(); static ModeManager *instance() { return m_instance; } @@ -69,10 +70,13 @@ public: void addAction(Command *command, int priority, QMenu *menu = 0); void addWidget(QWidget *widget); void updateModeNameIcon(IMode *mode, const QIcon &icon, const QString &label); + QVector modes() const { return m_modes; } + void reorderModes(QMap priorities); signals: void currentModeAboutToChange(Core::IMode *mode); void currentModeChanged(Core::IMode *mode); + void newModeOrder(QVector modes); public slots: void activateMode(const QString &id); @@ -84,19 +88,22 @@ private slots: void currentTabAboutToChange(int index); void currentTabChanged(int index); void updateModeToolTip(); + void tabMoved(int from, int to); private: int indexOf(const QString &id) const; + void setDefaultKeyshortcuts(); static ModeManager *m_instance; Internal::MainWindow *m_mainWindow; - Internal::FancyTabWidget *m_modeStack; - Internal::FancyActionBar *m_actionBar; + MyTabWidget *m_modeStack; QMap m_actions; QVector m_modes; QVector m_modeShortcuts; QSignalMapper *m_signalMapper; QList m_addedContexts; + QList m_tabOrder; + bool m_isReprioritizing; }; } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp index 611f3c0ad..183ffa185 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp @@ -73,57 +73,6 @@ void SplitterOrView::mousePressEvent(QMouseEvent *e) } } -//void SplitterOrView::paintEvent(QPaintEvent *event) -//{ -// if (m_uavGadgetManager->currentSplitterOrView() != this) -// return; -// -// if (!m_view) -// return; -// -// if (hasGadget()) -// return; -// -// if (m_uavGadgetManager->toolbarsShown()) -// return; -// -// // Discreet indication where an uavGadget would be if there is none -// QPainter painter(this); -// painter.setRenderHint(QPainter::Antialiasing, true); -// painter.setPen(Qt::NoPen); -// QColor shadeBrush(Qt::black); -// shadeBrush.setAlpha(25); -// painter.setBrush(shadeBrush); -// const int r = 3; -// painter.drawRoundedRect(rect().adjusted(r, r, -r, -r), r * 2, r * 2); -// -//#if 0 -// if (hasFocus()) { -//#ifdef Q_WS_MAC -// // With QMacStyle, we have to draw our own focus rect, since I didn't find -// // a way to draw the nice mac focus rect _inside_ this widget -// if (qobject_cast(style())) { -// painter.setPen(Qt::DotLine); -// painter.setBrush(Qt::NoBrush); -// painter.setOpacity(0.75); -// painter.drawRect(rect()); -// } else { -//#endif -// QStyleOptionFocusRect option; -// option.initFrom(this); -// option.backgroundColor = palette().color(QPalette::Background); -// -// // Some styles require a certain state flag in order to draw the focus rect -// option.state |= QStyle::State_KeyboardFocusChange; -// -// style()->drawPrimitive(QStyle::PE_FrameFocusRect, &option, &painter); -//#ifdef Q_WS_MAC -// } -//#endif -// } -//#endif -//} - /* Contract: return SplitterOrView that is not splitter, or 0 if not found. * Implications: must not return SplitterOrView that is splitter. */ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp index 71078ffee..c581a052c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.cpp @@ -29,7 +29,6 @@ #include "uavgadgetmanager.h" #include "uavgadgetview.h" #include "splitterorview.h" -#include "uavgadgetmode.h" #include "uavgadgetinstancemanager.h" #include "iuavgadgetfactory.h" #include "iuavgadget.h" @@ -79,229 +78,76 @@ static inline ExtensionSystem::PluginManager *pluginManager() //===================UAVGadgetManager===================== -//UAVGadgetManagerPlaceHolder *UAVGadgetManagerPlaceHolder::m_current = 0; - -UAVGadgetManagerPlaceHolder::UAVGadgetManagerPlaceHolder(Core::Internal::UAVGadgetMode *mode, QWidget *parent) - : QWidget(parent), - m_uavGadgetMode(mode), - m_current(0) -{ - m_mode = dynamic_cast(mode); - setLayout(new QVBoxLayout); - layout()->setMargin(0); - connect(Core::ModeManager::instance(), SIGNAL(currentModeChanged(Core::IMode *)), - this, SLOT(currentModeChanged(Core::IMode *))); - - currentModeChanged(Core::ModeManager::instance()->currentMode()); -} - -UAVGadgetManagerPlaceHolder::~UAVGadgetManagerPlaceHolder() -{ - if (m_current == this) { - m_uavGadgetMode->uavGadgetManager()->setParent(0); - m_uavGadgetMode->uavGadgetManager()->hide(); - } -} - -void UAVGadgetManagerPlaceHolder::currentModeChanged(Core::IMode *mode) -{ - UAVGadgetManager *gm = m_uavGadgetMode->uavGadgetManager(); - if (m_current == this) { - m_current = 0; - gm->setParent(0); - gm->hide(); - } - if (m_mode == mode) { - m_current = this; - layout()->addWidget(gm); - gm->showToolbars(gm->toolbarsShown()); - gm->show(); - } -} - -// ---------------- UAVGadgetManager - -namespace Core { - - -struct UAVGadgetManagerPrivate { - explicit UAVGadgetManagerPrivate(ICore *core, QWidget *parent); - ~UAVGadgetManagerPrivate(); - - // The root splitter or view. - QPointer m_splitterOrView; - - // The gadget which is currently 'active'. - QPointer m_currentGadget; - - QPointer m_core; - - QPointer m_coreListener; - - // actions - static QAction *m_showToolbarsAction; - static QAction *m_splitAction; - static QAction *m_splitSideBySideAction; - static QAction *m_removeCurrentSplitAction; - static QAction *m_removeAllSplitsAction; - static QAction *m_gotoOtherSplitAction; -}; -} - -QAction *UAVGadgetManagerPrivate::m_showToolbarsAction = 0; -QAction *UAVGadgetManagerPrivate::m_splitAction = 0; -QAction *UAVGadgetManagerPrivate::m_splitSideBySideAction = 0; -QAction *UAVGadgetManagerPrivate::m_removeCurrentSplitAction = 0; -QAction *UAVGadgetManagerPrivate::m_removeAllSplitsAction = 0; -QAction *UAVGadgetManagerPrivate::m_gotoOtherSplitAction = 0; - -UAVGadgetManagerPrivate::UAVGadgetManagerPrivate(ICore *core, QWidget *parent) : +UAVGadgetManager::UAVGadgetManager(ICore *core, QString name, QIcon icon, int priority, QString uniqueName, QWidget *parent) : + m_showToolbars(true), m_splitterOrView(0), m_currentGadget(0), m_core(core), - m_coreListener(0) -{ - Q_UNUSED(parent); -} - -UAVGadgetManagerPrivate::~UAVGadgetManagerPrivate() -{ -} - -UAVGadgetManager::UAVGadgetManager(ICore *core, QWidget *parent) : - QWidget(parent), - m_showToolbars(false), - m_d(new UAVGadgetManagerPrivate(core, parent)), - m_uavGadgetMode(0) + m_name(name), + m_icon(icon), + m_priority(priority), + m_widget(new QWidget(parent)) { - connect(m_d->m_core, SIGNAL(contextAboutToChange(Core::IContext *)), + // checking that the mode name is unique gives harmless + // warnings on the console output + ModeManager *modeManager = ModeManager::instance(); + if (!modeManager->mode(uniqueName)) { + m_uniqueName = uniqueName; + } else { + // this shouldn't happen + m_uniqueName = uniqueName + QString::number(quint64(this)); + } + m_uniqueNameBA = m_uniqueName.toLatin1(); + m_uniqueModeName = m_uniqueNameBA.data(); + + connect(m_core, SIGNAL(contextAboutToChange(Core::IContext *)), this, SLOT(handleContextChange(Core::IContext *))); - const QList uavGadgetManagerContext = - QList() << m_d->m_core->uniqueIDManager()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); - ActionManager *am = m_d->m_core->actionManager(); - - //Window Menu - ActionContainer *mwindow = am->actionContainer(Constants::M_WINDOW); - Command *cmd; - - // The actions m_d->m_showToolbarsAction etc are common to all instances of UAVGadgetManager - // which means that they share the menu items/signals in the Window menu. - // That is, they all connect their slots to the same signal and have to check in the slot - // if the current mode is their mode, otherwise they just ignore the signal. - // The first UAVGadgetManager creates the actions, and the following just use them - // (This also implies that they share the same context.) - if (m_d->m_showToolbarsAction == 0) - { - //Window menu separators - QAction *tmpaction = new QAction(this); - tmpaction->setSeparator(true); - cmd = am->registerAction(tmpaction, QLatin1String("OpenPilot.Window.Sep.Split"), uavGadgetManagerContext); - mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); - - m_d->m_showToolbarsAction = new QAction(tr("Edit Gadgets Mode"), this); - m_d->m_showToolbarsAction->setCheckable(true); - cmd = am->registerAction(m_d->m_showToolbarsAction, Constants::HIDE_TOOLBARS, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("Ctrl+Shift+F10"))); - mwindow->addAction(cmd, Constants::G_WINDOW_HIDE_TOOLBAR); - - //Window menu separators - QAction *tmpaction2 = new QAction(this); - tmpaction2->setSeparator(true); - cmd = am->registerAction(tmpaction2, QLatin1String("OpenPilot.Window.Sep.Split2"), uavGadgetManagerContext); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_showToolbarsAction, SIGNAL(triggered(bool)), this, SLOT(showToolbars(bool))); - -#ifdef Q_WS_MAC - QString prefix = tr("Meta+Shift"); -#else - QString prefix = tr("Ctrl+Shift"); -#endif - - if (m_d->m_splitAction == 0) - { - m_d->m_splitAction = new QAction(tr("Split"), this); - cmd = am->registerAction(m_d->m_splitAction, Constants::SPLIT, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("%1+Down").arg(prefix))); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_splitAction, SIGNAL(triggered()), this, SLOT(split())); - - if (m_d->m_splitSideBySideAction == 0) - { - m_d->m_splitSideBySideAction = new QAction(tr("Split Side by Side"), this); - cmd = am->registerAction(m_d->m_splitSideBySideAction, Constants::SPLIT_SIDE_BY_SIDE, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("%1+Right").arg(prefix))); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_splitSideBySideAction, SIGNAL(triggered()), this, SLOT(splitSideBySide())); - - if (m_d->m_removeCurrentSplitAction == 0) - { - m_d->m_removeCurrentSplitAction = new QAction(tr("Close Current View"), this); - cmd = am->registerAction(m_d->m_removeCurrentSplitAction, Constants::REMOVE_CURRENT_SPLIT, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("%1+C").arg(prefix))); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_removeCurrentSplitAction, SIGNAL(triggered()), this, SLOT(removeCurrentSplit())); - - if (m_d->m_removeAllSplitsAction == 0) - { - m_d->m_removeAllSplitsAction = new QAction(tr("Close All Other Views"), this); - cmd = am->registerAction(m_d->m_removeAllSplitsAction, Constants::REMOVE_ALL_SPLITS, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("%1+A").arg(prefix))); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_removeAllSplitsAction, SIGNAL(triggered()), this, SLOT(removeAllSplits())); - - if (m_d->m_gotoOtherSplitAction == 0) - { - m_d->m_gotoOtherSplitAction = new QAction(tr("Goto Next View"), this); - cmd = am->registerAction(m_d->m_gotoOtherSplitAction, Constants::GOTO_OTHER_SPLIT, uavGadgetManagerContext); - cmd->setDefaultKeySequence(QKeySequence(tr("%1+N").arg(prefix))); - mwindow->addAction(cmd, Constants::G_WINDOW_SPLIT); - } - connect(m_d->m_gotoOtherSplitAction, SIGNAL(triggered()), this, SLOT(gotoOtherSplit())); + connect(modeManager, SIGNAL(currentModeChanged(Core::IMode*)), + this, SLOT(modeChanged(Core::IMode*))); // other setup - m_d->m_splitterOrView = new SplitterOrView(this, 0); + m_splitterOrView = new SplitterOrView(this, 0); + // SplitterOrView with 0 as gadget calls our setCurrentGadget, which relies on currentSplitterOrView(), // which needs our m_splitterorView to be set, which isn't set yet at that time. // So directly set our currentGadget to 0, and do it again. - m_d->m_currentGadget = 0; - setCurrentGadget(m_d->m_splitterOrView->view()->gadget()); + m_currentGadget = 0; + setCurrentGadget(m_splitterOrView->view()->gadget()); - QHBoxLayout *layout = new QHBoxLayout(this); + QHBoxLayout *layout = new QHBoxLayout(m_widget); layout->setMargin(0); layout->setSpacing(0); - layout->addWidget(m_d->m_splitterOrView); + layout->addWidget(m_splitterOrView); showToolbars(m_showToolbars); - updateActions(); } UAVGadgetManager::~UAVGadgetManager() { - if (m_d->m_core) { - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - if (m_d->m_coreListener) { - pm->removeObject(m_d->m_coreListener); - delete m_d->m_coreListener; - } - } - delete m_d; +} + +QList UAVGadgetManager::context() const +{ + static QList contexts = QList() << + UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); + return contexts; +} + +void UAVGadgetManager::modeChanged(Core::IMode *mode) +{ + if (mode != this) + return; + + m_currentGadget->widget()->setFocus(); + showToolbars(toolbarsShown()); } void UAVGadgetManager::init() { QList context; - context << m_d->m_core->uniqueIDManager()->uniqueIdentifier("OpenPilot.UAVGadgetManager"); - - m_d->m_coreListener = new UAVGadgetClosingCoreListener(this); - - pluginManager()->addObject(m_d->m_coreListener); + context << m_core->uniqueIDManager()->uniqueIdentifier("OpenPilot.UAVGadgetManager"); } void UAVGadgetManager::handleContextChange(Core::IContext *context) @@ -311,16 +157,15 @@ void UAVGadgetManager::handleContextChange(Core::IContext *context) IUAVGadget *uavGadget = context ? qobject_cast(context) : 0; if (uavGadget) setCurrentGadget(uavGadget); - updateActions(); } void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) { - if (m_d->m_currentGadget == uavGadget) + if (m_currentGadget == uavGadget) return; SplitterOrView *oldView = currentSplitterOrView(); - m_d->m_currentGadget = uavGadget; + m_currentGadget = uavGadget; SplitterOrView *view = currentSplitterOrView(); if (oldView != view) { if (oldView) @@ -330,8 +175,6 @@ void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) } uavGadget->widget()->setFocus(); emit currentGadgetChanged(uavGadget); - updateActions(); -// emit currentGadgetChanged(uavGadget); } /* Contract: return current SplitterOrView. @@ -339,17 +182,17 @@ void UAVGadgetManager::setCurrentGadget(IUAVGadget *uavGadget) */ Core::Internal::SplitterOrView *UAVGadgetManager::currentSplitterOrView() const { - if (!m_d->m_splitterOrView) // this is only for startup + if (!m_splitterOrView) // this is only for startup return 0; - SplitterOrView *view = m_d->m_currentGadget ? - m_d->m_splitterOrView->findView(m_d->m_currentGadget) : + SplitterOrView *view = m_currentGadget ? + m_splitterOrView->findView(m_currentGadget) : 0; return view; } IUAVGadget *UAVGadgetManager::currentGadget() const { - return m_d->m_currentGadget; + return m_currentGadget; } void UAVGadgetManager::emptyView(Core::Internal::UAVGadgetView *view) @@ -369,10 +212,10 @@ void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) { if (!view) return; - SplitterOrView *splitterOrView = m_d->m_splitterOrView->findView(view); + SplitterOrView *splitterOrView = m_splitterOrView->findView(view); Q_ASSERT(splitterOrView); Q_ASSERT(splitterOrView->view() == view); - if (splitterOrView == m_d->m_splitterOrView) + if (splitterOrView == m_splitterOrView) return; IUAVGadget *gadget = view->gadget(); @@ -380,7 +223,7 @@ void UAVGadgetManager::closeView(Core::Internal::UAVGadgetView *view) UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); im->removeGadget(gadget); - SplitterOrView *splitter = m_d->m_splitterOrView->findSplitter(splitterOrView); + SplitterOrView *splitter = m_splitterOrView->findSplitter(splitterOrView); Q_ASSERT(splitterOrView->hasGadget() == false); Q_ASSERT(splitter->isSplitter() == true); splitterOrView->hide(); @@ -398,7 +241,7 @@ void UAVGadgetManager::addGadgetToContext(IUAVGadget *gadget) { if (!gadget) return; - m_d->m_core->addContextObject(gadget); + m_core->addContextObject(gadget); // emit uavGadgetOpened(uavGadget); } @@ -407,27 +250,39 @@ void UAVGadgetManager::removeGadget(IUAVGadget *gadget) { if (!gadget) return; - m_d->m_core->removeContextObject(qobject_cast(gadget)); + m_core->removeContextObject(qobject_cast(gadget)); } void UAVGadgetManager::ensureUAVGadgetManagerVisible() { - if (!isVisible()) - m_d->m_core->modeManager()->activateMode(m_uavGadgetMode->uniqueModeName()); + if (!m_widget->isVisible()) + m_core->modeManager()->activateMode(this->uniqueModeName()); } -void UAVGadgetManager::updateActions() +void UAVGadgetManager::showToolbars(bool show) { - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) + if (m_core->modeManager()->currentMode() != this) return; - if (!m_d->m_splitterOrView) // this is only for startup + + m_showToolbars = show; + SplitterOrView *next = m_splitterOrView->findFirstView(); + do { + next->view()->showToolbar(show); + next = m_splitterOrView->findNextView(next); + } while (next); + + updateUavGadgetMenus(); +} + +void UAVGadgetManager::updateUavGadgetMenus() +{ + if (m_core->modeManager()->currentMode() != this) + return; + if (!m_splitterOrView) // this is only for startup return; // Splitting is only possible when the toolbars are shown - bool shown = m_d->m_showToolbarsAction->isChecked(); - bool hasSplitter = m_d->m_splitterOrView->isSplitter(); - m_d->m_removeCurrentSplitAction->setEnabled(shown && hasSplitter); - m_d->m_removeAllSplitsAction->setEnabled(shown && hasSplitter); - m_d->m_gotoOtherSplitAction->setEnabled(shown && hasSplitter); + bool hasSplitter = m_splitterOrView->isSplitter(); + emit showUavGadgetMenus(m_showToolbars, hasSplitter); } void UAVGadgetManager::saveState(QSettings* qSettings) const @@ -435,7 +290,7 @@ void UAVGadgetManager::saveState(QSettings* qSettings) const qSettings->setValue("version","UAVGadgetManagerV1"); qSettings->setValue("showToolbars",m_showToolbars); qSettings->beginGroup("splitter"); - m_d->m_splitterOrView->saveState(qSettings); + m_splitterOrView->saveState(qSettings); qSettings->endGroup(); } @@ -444,8 +299,8 @@ bool UAVGadgetManager::restoreState(QSettings* qSettings) removeAllSplits(); UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager(); - IUAVGadget *gadget = m_d->m_splitterOrView->view()->gadget(); - emptyView(m_d->m_splitterOrView->view()); + IUAVGadget *gadget = m_splitterOrView->view()->gadget(); + emptyView(m_splitterOrView->view()); im->removeGadget(gadget); QString version = qSettings->value("version").toString(); @@ -458,7 +313,7 @@ bool UAVGadgetManager::restoreState(QSettings* qSettings) QApplication::setOverrideCursor(Qt::WaitCursor); qSettings->beginGroup("splitter"); - m_d->m_splitterOrView->restoreState(qSettings); + m_splitterOrView->restoreState(qSettings); qSettings->endGroup(); QApplication::restoreOverrideCursor(); @@ -468,7 +323,7 @@ bool UAVGadgetManager::restoreState(QSettings* qSettings) void UAVGadgetManager::saveSettings(QSettings *qs) { qs->beginGroup("UAVGadgetManager"); - qs->beginGroup(m_uavGadgetMode->uniqueModeName()); + qs->beginGroup(this->uniqueModeName()); // Make sure the old tree is wiped. qs->remove(""); @@ -488,16 +343,15 @@ void UAVGadgetManager::readSettings(QSettings *qs) } qs->beginGroup(uavGadgetManagerRootKey); - if(!qs->childGroups().contains(m_uavGadgetMode->uniqueModeName())) { + if(!qs->childGroups().contains(uniqueModeName())) { qs->endGroup(); return; } - qs->beginGroup(m_uavGadgetMode->uniqueModeName()); + qs->beginGroup(uniqueModeName()); restoreState(qs); showToolbars(m_showToolbars); - updateActions(); qs->endGroup(); qs->endGroup(); @@ -505,18 +359,19 @@ void UAVGadgetManager::readSettings(QSettings *qs) void UAVGadgetManager::split(Qt::Orientation orientation) { - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) + if (m_core->modeManager()->currentMode() != this) return; - IUAVGadget *uavGadget = m_d->m_currentGadget; + IUAVGadget *uavGadget = m_currentGadget; Q_ASSERT(uavGadget); SplitterOrView *view = currentSplitterOrView(); Q_ASSERT(view); view->split(orientation); - SplitterOrView *sor = m_d->m_splitterOrView->findView(uavGadget); - SplitterOrView *next = m_d->m_splitterOrView->findNextView(sor); + SplitterOrView *sor = m_splitterOrView->findView(uavGadget); + SplitterOrView *next = m_splitterOrView->findNextView(sor); setCurrentGadget(next->gadget()); + updateUavGadgetMenus(); } void UAVGadgetManager::split() @@ -531,37 +386,38 @@ void UAVGadgetManager::splitSideBySide() void UAVGadgetManager::removeCurrentSplit() { - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) + if (m_core->modeManager()->currentMode() != this) return; SplitterOrView *viewToClose = currentSplitterOrView(); - if (viewToClose == m_d->m_splitterOrView) + if (viewToClose == m_splitterOrView) return; closeView(viewToClose->view()); + updateUavGadgetMenus(); } // Removes all gadgets and splits in the workspace, except the current/active gadget. void UAVGadgetManager::removeAllSplits() { - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) + if (m_core->modeManager()->currentMode() != this) return; - if (!m_d->m_splitterOrView->isSplitter()) + if (!m_splitterOrView->isSplitter()) return; // Use a QPointer, just in case we accidently delete the gadget we want to keep. - QPointer currentGadget = m_d->m_currentGadget; + QPointer currentGadget = m_currentGadget; Q_ASSERT(currentGadget); - QList gadgets = m_d->m_splitterOrView->gadgets(); + QList gadgets = m_splitterOrView->gadgets(); Q_ASSERT(gadgets.count(currentGadget) == 1); gadgets.removeOne(currentGadget); // Remove all splits and their gadgets, then create a new view with the current gadget. - m_d->m_splitterOrView->unsplitAll(currentGadget); + m_splitterOrView->unsplitAll(currentGadget); // Zeroing the current gadget means setCurrentGadget will do something when we call it. - m_d->m_currentGadget = 0; + m_currentGadget = 0; setCurrentGadget(currentGadget); // Remove all other gadgets from the instance manager. @@ -569,53 +425,21 @@ void UAVGadgetManager::removeAllSplits() foreach (IUAVGadget *g, gadgets) { im->removeGadget(g); } + updateUavGadgetMenus(); } void UAVGadgetManager::gotoOtherSplit() { - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) + if (m_core->modeManager()->currentMode() != this) return; - if (m_d->m_splitterOrView->isSplitter()) { + if (m_splitterOrView->isSplitter()) { SplitterOrView *currentView = currentSplitterOrView(); - SplitterOrView *view = m_d->m_splitterOrView->findNextView(currentView); + SplitterOrView *view = m_splitterOrView->findNextView(currentView); if (!view) - view = m_d->m_splitterOrView->findFirstView(); + view = m_splitterOrView->findFirstView(); if (view) { setCurrentGadget(view->gadget()); } } } - -void UAVGadgetManager::showToolbars(bool show) -{ - if (m_d->m_core->modeManager()->currentMode() != m_uavGadgetMode) - return; - - m_d->m_showToolbarsAction->setChecked(show); - m_showToolbars = show; - SplitterOrView *next = m_d->m_splitterOrView->findFirstView(); - do { - next->view()->showToolbar(show); - next = m_d->m_splitterOrView->findNextView(next); - } while (next); - - m_d->m_splitAction->setEnabled(show); - m_d->m_splitSideBySideAction->setEnabled(show); - m_d->m_removeCurrentSplitAction->setEnabled(show); - m_d->m_removeAllSplitsAction->setEnabled(show); - m_d->m_gotoOtherSplitAction->setEnabled(show); -} -//===================UAVGadgetClosingCoreListener====================== - -UAVGadgetClosingCoreListener::UAVGadgetClosingCoreListener(UAVGadgetManager *em) - : m_em(em) -{ -} - -bool UAVGadgetClosingCoreListener::coreAboutToClose() -{ - // Do not ask for files to save. - // MainWindow::closeEvent has already done that. - return true; -} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h index 1eea66b2a..3764fa1fb 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetmanager.h @@ -32,14 +32,15 @@ #include "../core_global.h" #include +#include #include #include #include #include +#include QT_BEGIN_NAMESPACE -class QSettings; class QModelIndex; QT_END_NAMESPACE @@ -48,49 +49,33 @@ namespace Core { class IContext; class ICore; class IUAVGadget; -class IMode; - -struct UAVGadgetManagerPrivate; namespace Internal { -class UAVGadgetMode; class UAVGadgetView; class SplitterOrView; -class UAVGadgetClosingCoreListener; - - } // namespace Internal -class CORE_EXPORT UAVGadgetManagerPlaceHolder : public QWidget -{ - Q_OBJECT -public: - UAVGadgetManagerPlaceHolder(Core::Internal::UAVGadgetMode *mode, QWidget *parent = 0); - ~UAVGadgetManagerPlaceHolder(); -// static UAVGadgetManagerPlaceHolder* current(); -private slots: - void currentModeChanged(Core::IMode *); -private: - Core::IMode *m_mode; - Core::Internal::UAVGadgetMode *m_uavGadgetMode; - UAVGadgetManagerPlaceHolder* m_current; -}; - - -class CORE_EXPORT UAVGadgetManager : public QWidget +class CORE_EXPORT UAVGadgetManager : public IMode { Q_OBJECT public: - explicit UAVGadgetManager(ICore *core, QWidget *parent); + explicit UAVGadgetManager(ICore *core, QString name, QIcon icon, int priority, QString uniqueName, QWidget *parent); virtual ~UAVGadgetManager(); + + // IMode + QString name() const { return m_name; } + QIcon icon() const { return m_icon; } + int priority() const { return m_priority; } + void setPriority(int priority) { m_priority = priority; } + const char *uniqueModeName() const { return m_uniqueModeName; } + QList context() const; + void init(); - // setUAVGadgetMode should be called exactly once - // right after the mode has been created, and never thereafter - void setUAVGadgetMode(Core::Internal::UAVGadgetMode *mode) { m_uavGadgetMode = mode; } + QWidget *widget() { return m_widget; } void ensureUAVGadgetManagerVisible(); @@ -105,10 +90,14 @@ public: signals: void currentGadgetChanged(IUAVGadget *gadget); + void showUavGadgetMenus(bool show, bool hasSplitter); + void updateSplitMenus(bool hasSplitter); private slots: void handleContextChange(Core::IContext *context); - void updateActions(); + void updateUavGadgetMenus(); + void modeChanged(Core::IMode *mode); + public slots: void split(Qt::Orientation orientation); @@ -120,16 +109,25 @@ public slots: void showToolbars(bool show); private: + void setCurrentGadget(IUAVGadget *gadget); void addGadgetToContext(IUAVGadget *gadget); void removeGadget(IUAVGadget *gadget); - void setCurrentGadget(IUAVGadget *gadget); void closeView(Core::Internal::UAVGadgetView *view); void emptyView(Core::Internal::UAVGadgetView *view); Core::Internal::SplitterOrView *currentSplitterOrView() const; bool m_showToolbars; - UAVGadgetManagerPrivate *m_d; - Core::Internal::UAVGadgetMode *m_uavGadgetMode; + Core::Internal::SplitterOrView *m_splitterOrView; + Core::IUAVGadget *m_currentGadget; + Core::ICore *m_core; + + QString m_name; + QIcon m_icon; + int m_priority; + QString m_uniqueName; + QByteArray m_uniqueNameBA; + const char* m_uniqueModeName; + QWidget *m_widget; friend class Core::Internal::SplitterOrView; friend class Core::Internal::UAVGadgetView; @@ -137,26 +135,4 @@ private: } // namespace Core - -//===================UAVGadgetClosingCoreListener====================== - -namespace Core { -namespace Internal { - -class UAVGadgetClosingCoreListener : public ICoreListener -{ - Q_OBJECT - -public: - UAVGadgetClosingCoreListener(UAVGadgetManager *em); - bool uavGadgetAboutToClose(IUAVGadget *gadget); - bool coreAboutToClose(); - -private: - UAVGadgetManager *m_em; -}; - -} // namespace Internal -} // namespace Core - #endif // UAVGADGETMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.cpp deleted file mode 100644 index eeb6df7a3..000000000 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavgadgetmode.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup CorePlugin Core Plugin - * @{ - * @brief The Core GCS plugin - *****************************************************************************/ -/* - * 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 "uavgadgetmode.h" -#include "uavgadgetmanager.h" -#include "coreconstants.h" -#include "modemanager.h" -#include "uniqueidmanager.h" -#include "minisplitter.h" -#include "outputpane.h" -#include "rightpane.h" -#include "iuavgadget.h" - -#include -#include -#include -#include -#include - -using namespace Core; -using namespace Core::Internal; - -UAVGadgetMode::UAVGadgetMode(UAVGadgetManager *uavGadgetManager, QString name, QIcon icon, int priority, QString uniqueName) : - m_uavGadgetManager(uavGadgetManager), - m_name(name), - m_icon(icon), - m_widget(new QWidget), - m_priority(priority), - m_layout(new QVBoxLayout) -{ - m_layout->setSpacing(0); - m_layout->setMargin(0); - m_widget->setLayout(m_layout); - m_layout->insertWidget(0, new Core::UAVGadgetManagerPlaceHolder(this)); - - ModeManager *modeManager = ModeManager::instance(); - // checking that the mode name is unique gives harmless - // warnings on the console output - if (!modeManager->mode(uniqueName)) { - m_uniqueName = uniqueName; - } else { - // this shouldn't happen - m_uniqueName = uniqueName + QString::number(quint64(this)); - } - m_uniqueNameBA = m_uniqueName.toLatin1(); - m_uniqueNameC = m_uniqueNameBA.data(); - connect(modeManager, SIGNAL(currentModeChanged(Core::IMode*)), - this, SLOT(grabUAVGadgetManager(Core::IMode*))); - m_widget->setFocusProxy(m_uavGadgetManager); -} - -UAVGadgetMode::~UAVGadgetMode() -{ - // TODO: see if this leftover from Qt Creator still applies - // Make sure the uavGadget manager does not get deleted - m_uavGadgetManager->setParent(0); - delete m_widget; -} - -QString UAVGadgetMode::name() const -{ - return m_name; -} - -void UAVGadgetMode::setName(QString name) -{ - m_name = name; -} - -QIcon UAVGadgetMode::icon() const -{ - return m_icon; -} - -void UAVGadgetMode::setIcon(QIcon icon) -{ - m_icon = icon; -} - -int UAVGadgetMode::priority() const -{ - return m_priority; -} - -QWidget* UAVGadgetMode::widget() -{ - return m_widget; -} - -const char* UAVGadgetMode::uniqueModeName() const -{ - return m_uniqueNameC; -} - -QList UAVGadgetMode::context() const -{ - static QList contexts = QList() << - UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGET_MODE) << - UniqueIDManager::instance()->uniqueIdentifier(Constants::C_UAVGADGETMANAGER); - return contexts; -} - -void UAVGadgetMode::grabUAVGadgetManager(Core::IMode *mode) -{ - if (mode != this) - return; - - if (m_uavGadgetManager->currentGadget()) - m_uavGadgetManager->currentGadget()->widget()->setFocus(); -} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.h b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.h deleted file mode 100644 index 5aac09221..000000000 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmode.h +++ /dev/null @@ -1,87 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavgadgetmode.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup CorePlugin Core Plugin - * @{ - * @brief The Core GCS plugin - *****************************************************************************/ -/* - * 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 UAVGADGETMODE_H -#define UAVGADGETMODE_H - -#include - -#include -#include - -QT_BEGIN_NAMESPACE -class QSplitter; -class QWidget; -class QIcon; -class QVBoxLayout; -QT_END_NAMESPACE - -namespace Core { - -class UAVGadgetManager; - -namespace Internal { - -class UAVGadgetMode : public Core::IMode -{ - Q_OBJECT - -public: - UAVGadgetMode(UAVGadgetManager *uavGadgetManager, QString name, QIcon icon, int priority, QString uniqueName); - ~UAVGadgetMode(); - - // IMode - QString name() const; - QIcon icon() const; - int priority() const; - QWidget* widget(); - const char* uniqueModeName() const; - QList context() const; - UAVGadgetManager* uavGadgetManager() const { return m_uavGadgetManager; } - void setName(QString name); - void setIcon(QIcon icon); - -private slots: - void grabUAVGadgetManager(Core::IMode *mode); - -private: - UAVGadgetManager *m_uavGadgetManager; - QString m_name; - QIcon m_icon; - QWidget *m_widget; - int m_priority; - QVBoxLayout *m_layout; - QString m_uniqueName; - QByteArray m_uniqueNameBA; - const char *m_uniqueNameC; -}; - -} // namespace Internal -} // namespace Core - -#endif // UAVGADGETMODE_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.cpp deleted file mode 100644 index 4acc08683..000000000 --- a/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/** - ****************************************************************************** - * - * @file viewmanager.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup CorePlugin Core Plugin - * @{ - * @brief The Core GCS plugin - *****************************************************************************/ -/* - * 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 "viewmanager.h" - -#include "coreconstants.h" -#include "mainwindow.h" -#include "uniqueidmanager.h" -#include "iview.h" - -#include -#include -#include -#include - -#include -#include -#include -#include - -using namespace Core; -using namespace Core::Internal; - -ViewManager::ViewManager(MainWindow *mainWnd) - : QObject(mainWnd), - m_mainWnd(mainWnd) -{ - for (int i = 0; i < 3; ++i) { - QWidget *w = new QWidget(); - m_mainWnd->statusBar()->insertPermanentWidget(i, w); - w->setLayout(new QHBoxLayout); - w->setVisible(true); - w->layout()->setMargin(0); - m_statusBarWidgets.append(w); - } - QLabel *l = new QLabel(); - m_mainWnd->statusBar()->insertPermanentWidget(3, l, 1); -} - -ViewManager::~ViewManager() -{ -} - -void ViewManager::init() -{ - connect(ExtensionSystem::PluginManager::instance(), SIGNAL(objectAdded(QObject*)), - this, SLOT(objectAdded(QObject*))); - connect(ExtensionSystem::PluginManager::instance(), SIGNAL(aboutToRemoveObject(QObject*)), - this, SLOT(aboutToRemoveObject(QObject*))); -} - -void ViewManager::objectAdded(QObject *obj) -{ - IView *view = Aggregation::query(obj); - if (!view) - return; - - QWidget *viewWidget = 0; - viewWidget = view->widget(); - m_statusBarWidgets.at(view->defaultPosition())->layout()->addWidget(viewWidget); - - m_viewMap.insert(view, viewWidget); - viewWidget->setObjectName(view->uniqueViewName()); - m_mainWnd->addContextObject(view); -} - -void ViewManager::aboutToRemoveObject(QObject *obj) -{ - IView *view = Aggregation::query(obj); - if (!view) - return; - m_mainWnd->removeContextObject(view); -} - -void ViewManager::readSettings(QSettings *settings) -{ - m_mainWnd->restoreState(settings->value(QLatin1String("ViewGroup_Default"), QByteArray()).toByteArray()); -} - -void ViewManager::saveSettings(QSettings *settings) -{ - settings->setValue(QLatin1String("ViewGroup_Default"), m_mainWnd->saveState()); -} - -IView *ViewManager::view(const QString &id) -{ - QList list = - ExtensionSystem::PluginManager::instance()->getObjects(); - foreach (IView *view, list) { - if (view->uniqueViewName() == id) - return view; - } - return 0; -} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.h deleted file mode 100644 index 6d6dee120..000000000 --- a/ground/openpilotgcs/src/plugins/coreplugin/viewmanager.h +++ /dev/null @@ -1,79 +0,0 @@ -/** - ****************************************************************************** - * - * @file viewmanager.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup CorePlugin Core Plugin - * @{ - * @brief The Core GCS plugin - *****************************************************************************/ -/* - * 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 VIEWMANAGER_H -#define VIEWMANAGER_H - -#include -#include - -QT_BEGIN_NAMESPACE -class QAction; -class QSettings; -class QMainWindow; -class QComboBox; -class QStackedWidget; -QT_END_NAMESPACE - -namespace Core { - -class IView; - -namespace Internal { - -class MainWindow; -class NavigationWidget; - -class ViewManager : public QObject -{ - Q_OBJECT - -public: - ViewManager(MainWindow *mainWnd); - ~ViewManager(); - - void init(); - void readSettings(QSettings *settings); - void saveSettings(QSettings *settings); - - IView * view(const QString & id); -private slots: - void objectAdded(QObject *obj); - void aboutToRemoveObject(QObject *obj); - -private: - QMap m_viewMap; - - MainWindow *m_mainWnd; - QList m_statusBarWidgets; -}; - -} // namespace Internal -} // namespace Core - -#endif // VIEWMANAGER_H diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp index 367a8af20..6bc2dc20c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp @@ -28,7 +28,7 @@ #include "workspacesettings.h" #include #include -#include +#include #include #include "ui_workspacesettings.h" @@ -95,26 +95,37 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent) m_currentIndex = 0; selectWorkspace(m_currentIndex); + if (0 <= m_tabBarPlacementIndex && m_tabBarPlacementIndex < m_page->comboBoxTabBarPlacement->count()) + m_page->comboBoxTabBarPlacement->setCurrentIndex(m_tabBarPlacementIndex); + m_page->checkBoxAllowTabMovement->setChecked(m_allowTabBarMovement); + return w; } void WorkspaceSettings::readSettings(QSettings* qs) { - m_iconNames.clear(); m_names.clear(); + m_iconNames.clear(); + m_modeNames.clear(); qs->beginGroup(QLatin1String("Workspace")); m_numberOfWorkspaces = qs->value(QLatin1String("NumberOfWorkspaces"), 2).toInt(); + m_previousNumberOfWorkspaces = m_numberOfWorkspaces; for (int i = 1; i <= MAX_WORKSPACES; ++i) { QString numberString = QString::number(i); QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; - const QString name = qs->value(defaultName, defaultName).toString(); - const QString iconName = qs->value(defaultIconName, ":/core/images/openpilot_logo_64.png").toString(); - m_iconNames.append(iconName); + QString name = qs->value(defaultName, defaultName).toString(); + QString iconName = qs->value(defaultIconName, ":/core/images/openpilot_logo_64.png").toString(); m_names.append(name); + m_iconNames.append(iconName); + m_modeNames.append(QString("Mode")+ QString::number(i)); } + m_tabBarPlacementIndex = qs->value(QLatin1String("TabBarPlacementIndex"), 1).toInt(); // 1 == "Bottom" + m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); qs->endGroup(); + QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; + emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } void WorkspaceSettings::saveSettings(QSettings* qs) @@ -122,12 +133,16 @@ void WorkspaceSettings::saveSettings(QSettings* qs) qs->beginGroup(QLatin1String("Workspace")); qs->setValue(QLatin1String("NumberOfWorkspaces"), m_numberOfWorkspaces); for (int i = 0; i < MAX_WORKSPACES; ++i) { + QString mode = QString("Mode")+ QString::number(i+1); + int j = m_modeNames.indexOf(mode); QString numberString = QString::number(i+1); QString defaultName = "Workspace" + numberString; QString defaultIconName = "Icon" + numberString; - qs->setValue(defaultName, m_names.at(i)); - qs->setValue(defaultIconName, m_iconNames.at(i)); + qs->setValue(defaultName, m_names.at(j)); + qs->setValue(defaultIconName, m_iconNames.at(j)); } + qs->setValue(QLatin1String("TabBarPlacementIndex"), m_tabBarPlacementIndex); + qs->setValue(QLatin1String("AllowTabBarMovement"), m_allowTabBarMovement); qs->endGroup(); } @@ -137,15 +152,23 @@ void WorkspaceSettings::apply() saveSettings(Core::ICore::instance()->settings()); + if (m_numberOfWorkspaces != m_previousNumberOfWorkspaces) { + Core::ICore::instance()->readMainSettings(Core::ICore::instance()->settings(), true); + m_previousNumberOfWorkspaces = m_numberOfWorkspaces; + } + ModeManager* modeManager = Core::ICore::instance()->modeManager(); for (int i = 0; i < MAX_WORKSPACES; ++i) { - Core::Internal::UAVGadgetMode *mode = - qobject_cast(modeManager->mode(modeName(i))); + IMode *baseMode = modeManager->mode(modeName(i)); + Core::UAVGadgetManager *mode = qobject_cast(baseMode); if (mode) { modeManager->updateModeNameIcon(mode, QIcon(iconName(i)), name(i)); } } - + m_tabBarPlacementIndex = m_page->comboBoxTabBarPlacement->currentIndex(); + m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); + QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; + emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } void WorkspaceSettings::finish() @@ -186,8 +209,8 @@ void WorkspaceSettings::selectWorkspace(int index, bool store) // write old values of workspace not shown anymore m_iconNames.replace(m_currentIndex, m_page->iconPathChooser->path()); m_names.replace(m_currentIndex, m_page->nameEdit->text()); - m_page->workspaceComboBox->setItemIcon(m_currentIndex, QIcon(m_page->iconPathChooser->path())); - m_page->workspaceComboBox->setItemText(m_currentIndex, m_page->nameEdit->text()); + m_page->workspaceComboBox->setItemIcon(m_currentIndex, QIcon(m_iconNames.at(m_currentIndex))); + m_page->workspaceComboBox->setItemText(m_currentIndex, m_names.at(m_currentIndex)); } // display current workspace @@ -196,3 +219,34 @@ void WorkspaceSettings::selectWorkspace(int index, bool store) m_page->nameEdit->setText(m_names.at(index)); m_currentIndex = index; } + +void WorkspaceSettings::newModeOrder(QVector modes) +{ + QList priorities; + QStringList modeNames; + for (int i = 0; i < modes.count(); ++i) { + Core::UAVGadgetManager *mode = qobject_cast(modes.at(i)); + if (mode) { + priorities.append(mode->priority()); + modeNames.append(mode->uniqueModeName()); + } + } + // Bubble sort + bool swapped = false; + do { + swapped = false; + for (int i = 0; i < m_names.count()-1; ++i) { + int j = i+1; + int p = modeNames.indexOf(m_modeNames.at(i)); + int q = modeNames.indexOf(m_modeNames.at(j)); + bool nonShowingMode = (p == -1 && q >=0); + bool pqBothFound = (p >= 0 && q >= 0); + if (nonShowingMode || (pqBothFound && (priorities.at(q) > priorities.at(p)))) { + m_names.swap(i, j); + m_iconNames.swap(i, j); + m_modeNames.swap(i, j); + swapped = true; + } + } + } while (swapped); +} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h index 15a98eaf2..ba790de56 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h @@ -31,12 +31,14 @@ #include #include #include +#include class QSettings; namespace Core { class ModeManager; + class IMode; namespace Internal { @@ -65,24 +67,29 @@ public: int numberOfWorkspaces() const { return m_numberOfWorkspaces;} QString iconName(int i) const { return m_iconNames.at(i);} QString name(int i) const { return m_names.at(i);} - QString modeName(int i) const { return QString("Mode")+ QString::number(i+1);} + QString modeName(int i) const { return m_modeNames.at(i);} signals: + void tabBarSettingsApplied(QTabWidget::TabPosition pos, bool movable); public slots: void selectWorkspace(int index, bool store = false); void numberOfWorkspacesChanged(int value); void textEdited(QString string); void iconChanged(); + void newModeOrder(QVector modes); + private: Ui::WorkspaceSettings *m_page; QStringList m_iconNames; QStringList m_names; + QStringList m_modeNames; int m_currentIndex; + int m_previousNumberOfWorkspaces; int m_numberOfWorkspaces; + int m_tabBarPlacementIndex; + bool m_allowTabBarMovement; static const int MAX_WORKSPACES; - - }; } // namespace Internal diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui index 8e19687df..1c646b693 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui @@ -6,7 +6,7 @@ 0 0 - 400 + 414 320 @@ -145,11 +145,71 @@ p, li { white-space: pre-wrap; } <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">Note:</span> A restart is needed for changes to number of workspaces to take effect.</p></body></html> - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Sans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">Note:</span> A restart is needed for changes to number of workspaces to take effect.</p></body></html> + + + + + + + + + + + Workspace panel + + + + + + Placement: + + + + + + + + + 1 + + + + Top + + + + + Bottom + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Allow reordering: + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp index a3bdebe42..ba6f35f19 100644 --- a/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/dial/dialgadgetwidget.cpp @@ -478,13 +478,13 @@ void DialGadgetWidget::setDialFont(QString fontProps) // this enables smooth rotation in rotateNeedles below void DialGadgetWidget::setNeedle1(double value) { if (rotateN1) { - needle1Target = 360*(value*n1Factor-n1MinValue)/(n1MaxValue-n1MinValue); + needle1Target = 360*(value*n1Factor)/(n1MaxValue-n1MinValue); } if (horizN1) { - needle1Target = (value*n1Factor-n1MinValue)/(n1MaxValue-n1MinValue); + needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); } if (vertN1) { - needle1Target = (value*n1Factor-n1MinValue)/(n1MaxValue-n1MinValue); + needle1Target = (value*n1Factor)/(n1MaxValue-n1MinValue); } if (!dialTimer.isActive()) dialTimer.start(); @@ -497,13 +497,13 @@ void DialGadgetWidget::setNeedle1(double value) { void DialGadgetWidget::setNeedle2(double value) { if (rotateN2) { - needle2Target = 360*(value*n2Factor-n2MinValue)/(n2MaxValue-n2MinValue); + needle2Target = 360*(value*n2Factor)/(n2MaxValue-n2MinValue); } if (horizN2) { - needle2Target = (value*n2Factor-n2MinValue)/(n2MaxValue-n2MinValue); + needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); } if (vertN2) { - needle2Target = (value*n2Factor-n2MinValue)/(n2MaxValue-n2MinValue); + needle2Target = (value*n2Factor)/(n2MaxValue-n2MinValue); } if (!dialTimer.isActive()) dialTimer.start(); @@ -517,13 +517,13 @@ void DialGadgetWidget::setNeedle2(double value) { void DialGadgetWidget::setNeedle3(double value) { if (rotateN3) { - needle3Target = 360*(value*n3Factor-n3MinValue)/(n3MaxValue-n3MinValue); + needle3Target = 360*(value*n3Factor)/(n3MaxValue-n3MinValue); } if (horizN3) { - needle3Target = (value*n3Factor-n3MinValue)/(n3MaxValue-n3MinValue); + needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); } if (vertN3) { - needle3Target = (value*n3Factor-n3MinValue)/(n3MaxValue-n3MinValue); + needle3Target = (value*n3Factor)/(n3MaxValue-n3MinValue); } if (!dialTimer.isActive()) dialTimer.start(); diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection.pri b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection.pri index fc15b3efc..74f3f4380 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnection.pri +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnection.pri @@ -1,3 +1,3 @@ include(ipconnection_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(IPconnection) +LIBS *= -l$$qtLibraryName(IPconnection) diff --git a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp index d5e85ccae..6333c4e98 100644 --- a/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp +++ b/ground/openpilotgcs/src/plugins/logging/loggingplugin.cpp @@ -76,11 +76,10 @@ QIODevice* LoggingConnection::openDevice(const QString &deviceName) if (logFile.isOpen()){ logFile.close(); } - QFileDialog * fd = new QFileDialog(); - fd->setAcceptMode(QFileDialog::AcceptOpen); - fd->setNameFilter("OpenPilot Log (*.opl)"); - connect(fd, SIGNAL(fileSelected(QString)), this, SLOT(startReplay(QString))); - fd->exec(); + QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open file"), QString(""), tr("OpenPilot Log (*.opl)")); + if (!fileName.isNull()) { + startReplay(fileName); + } return &logFile; } diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index e58608301..7a86f53be 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -64,12 +64,20 @@ plugin_uavobjectbrowser.depends = plugin_coreplugin plugin_uavobjectbrowser.depends += plugin_uavobjects SUBDIRS += plugin_uavobjectbrowser +!contains(QT_VERSION, ^4\\.8\\..*) { # ModelView UAVGadget plugin_modelview.subdir = modelview plugin_modelview.depends = plugin_coreplugin plugin_modelview.depends += plugin_uavobjects SUBDIRS += plugin_modelview +#Notify gadget +plugin_notify.subdir = notify +plugin_notify.depends = plugin_coreplugin +plugin_notify.depends += plugin_uavobjects +SUBDIRS += plugin_notify +} + #Uploader gadget plugin_uploader.subdir = uploader plugin_uploader.depends = plugin_coreplugin @@ -95,12 +103,6 @@ plugin_systemhealth.depends = plugin_coreplugin plugin_systemhealth.depends += plugin_uavobjects SUBDIRS += plugin_systemhealth -#Notify gadget -plugin_notify.subdir = notify -plugin_notify.depends = plugin_coreplugin -plugin_notify.depends += plugin_uavobjects -SUBDIRS += plugin_notify - #Config gadget plugin_config.subdir = config plugin_config.depends = plugin_coreplugin diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.pri b/ground/openpilotgcs/src/plugins/rawhid/rawhid.pri index 2761db5be..4982d3408 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.pri +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.pri @@ -1,3 +1,3 @@ include(rawhid_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(RawHID) +LIBS *= -l$$qtLibraryName(RawHID) diff --git a/ground/openpilotgcs/src/plugins/scope/scope.pri b/ground/openpilotgcs/src/plugins/scope/scope.pri index 99588122d..31dea2f41 100644 --- a/ground/openpilotgcs/src/plugins/scope/scope.pri +++ b/ground/openpilotgcs/src/plugins/scope/scope.pri @@ -1,3 +1,3 @@ -include(scope_dependencies.pri) - -LIBS *= -l$$qtLibraryTarget(ScopeGadget) +include(scope_dependencies.pri) + +LIBS *= -l$$qtLibraryName(ScopeGadget) diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serial.pri b/ground/openpilotgcs/src/plugins/serialconnection/serial.pri index d14f80e74..2b5291336 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serial.pri +++ b/ground/openpilotgcs/src/plugins/serialconnection/serial.pri @@ -1,3 +1,3 @@ include(serial_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(Serial) +LIBS *= -l$$qtLibraryName(Serial) diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp index 408b9e1ae..e62afb44a 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp @@ -165,8 +165,7 @@ void SerialConnection::closeDevice(const QString &deviceName) Q_UNUSED(deviceName); //we have to delete the serial connection we created if (serialHandle){ - serialHandle->close (); - delete(serialHandle); + serialHandle->deleteLater(); serialHandle = NULL; m_deviceOpened = false; } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pri b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pri index a6a378a1d..dbe866573 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pri +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pri @@ -1,6 +1,6 @@ -include(uavobjects_dependencies.pri) - -# Add the include path to the built-in uavobject include files. -INCLUDEPATH += $$PWD - -LIBS *= -l$$qtLibraryTarget(UAVObjects) +include(uavobjects_dependencies.pri) + +# Add the include path to the built-in uavobject include files. +INCLUDEPATH += $$PWD + +LIBS *= -l$$qtLibraryName(UAVObjects) diff --git a/ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.h b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h similarity index 96% rename from ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.h rename to ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h index 037ecede0..c44fb73c0 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/devicedescriptorstruct.h @@ -30,7 +30,7 @@ public: break; } } - deviceDescriptorStruct(); + deviceDescriptorStruct(){} }; #endif // DEVICEDESCRIPTORSTRUCT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pri b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pri index fde2f6de0..3890b284a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pri +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pri @@ -1,6 +1,6 @@ -include(uavobjectutil_dependencies.pri) - -# Add the include path to the built-in uavobject include files. -INCLUDEPATH += $$PWD - -LIBS *= -l$$qtLibraryTarget(UAVObjectUtil) +include(uavobjectutil_dependencies.pri) + +# Add the include path to the built-in uavobject include files. +INCLUDEPATH += $$PWD + +LIBS *= -l$$qtLibraryName(UAVObjectUtil) diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pro b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pro index 2776a2d25..92e490542 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pro +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutil.pro @@ -6,7 +6,8 @@ include(uavobjectutil_dependencies.pri) HEADERS += uavobjectutil_global.h \ uavobjectutilmanager.h \ - uavobjectutilplugin.h + uavobjectutilplugin.h \ + devicedescriptorstruct.h SOURCES += uavobjectutilmanager.cpp \ uavobjectutilplugin.cpp diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index 67d30aadb..b5070c269 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -652,4 +652,51 @@ int UAVObjectUtilManager::getTelemetrySerialPortSpeeds(QComboBox *comboBox) return 0; // OK } +deviceDescriptorStruct UAVObjectUtilManager::getBoardDescriptionStruct() +{ + deviceDescriptorStruct ret; + descriptionToStructure(getBoardDescription(),&ret); + return ret; +} + +bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescriptorStruct *struc) +{ + if (desc.startsWith("OpFw")) { + // This looks like a binary with a description at the end + /* + # 4 bytes: header: "OpFw" + # 4 bytes: GIT commit tag (short version of SHA1) + # 4 bytes: Unix timestamp of compile time + # 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. + # 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded + # ---- 40 bytes limit --- + # 20 bytes: SHA1 sum of the firmware. + # 40 bytes: free for now. + */ + + // Note: the ARM binary is big-endian: + quint32 gitCommitTag = desc.at(7)&0xFF; + for (int i=1;i<4;i++) { + gitCommitTag = gitCommitTag<<8; + gitCommitTag += desc.at(7-i) & 0xFF; + } + struc->gitTag=QString::number(gitCommitTag,16); + quint32 buildDate = desc.at(11)&0xFF; + for (int i=1;i<4;i++) { + buildDate = buildDate<<8; + buildDate += desc.at(11-i) & 0xFF; + } + struc->buildDate= QDateTime::fromTime_t(buildDate).toUTC().toString("yyyyMMdd HH:mm"); + QByteArray targetPlatform = desc.mid(12,2); + // TODO: check platform compatibility + QString dscText = QString(desc.mid(14,26)); + struc->boardType=(int)targetPlatform.at(0); + struc->boardRevision=(int)targetPlatform.at(1); + struc->description=dscText; + return true; + } + return false; + +} + // ****************************** diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 298374862..6c7782b25 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -35,14 +35,14 @@ #include "uavobjectmanager.h" #include "uavobject.h" #include "objectpersistence.h" - +#include "devicedescriptorstruct.h" #include #include #include #include #include #include - +#include class UAVOBJECTUTIL_EXPORT UAVObjectUtilManager: public QObject { Q_OBJECT @@ -65,6 +65,8 @@ public: QByteArray getBoardCPUSerial(); quint32 getFirmwareCRC(); QByteArray getBoardDescription(); + deviceDescriptorStruct getBoardDescriptionStruct(); + static bool descriptionToStructure(QByteArray desc,deviceDescriptorStruct * struc); UAVObjectManager* getObjectManager(); void saveObjectToSD(UAVObject *obj); diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp index f3c48c969..342b845e9 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.cpp @@ -86,7 +86,15 @@ bool UAVSettingsImportExportPlugin::initialize(const QStringList& args, QString ac->addAction(cmd, Core::Constants::G_FILE_SAVE); connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(importUAVSettings())); - return true; + cmd = am->registerAction(new QAction(this), + "UAVSettingsImportExportPlugin.UAVDataExport", + QList() << + Core::Constants::C_GLOBAL_ID); + cmd->action()->setText(tr("Export UAV Data...")); + ac->addAction(cmd, Core::Constants::G_FILE_SAVE); + connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(exportUAVData())); + + return true; } void UAVSettingsImportExportPlugin::extensionsInitialized() @@ -199,40 +207,35 @@ void UAVSettingsImportExportPlugin::importUAVSettings() } -// Slot called by the menu manager on user action -void UAVSettingsImportExportPlugin::exportUAVSettings() +// Create an XML document from UAVObject database +QString UAVSettingsImportExportPlugin::createXMLDocument( + const QString docName, const bool isSettings, const bool fullExport) { - // ask for file name - QString fileName; - QString filters = tr("UAVSettings XML files (*.uav)"); - - fileName = QFileDialog::getSaveFileName(0, tr("Save UAV Settings File As"), "", filters); - if (fileName.isEmpty()) { - return; - } - - bool fullExport = false; - // If the filename ends with .xml, we will do a full export, otherwise, a simple export - if (fileName.endsWith(".xml")) { - fullExport = true; - } else if (!fileName.endsWith(".uav")) { - fileName.append(".uav"); - } - // generate an XML first (used for all export formats as a formatted data source) ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - QDomDocument doc("UAVSettings"); - QDomElement root = doc.createElement("settings"); + QDomDocument doc(docName); + QDomElement root = doc.createElement(isSettings ? "settings" : "data"); doc.appendChild(root); + QDomElement versionInfo =doc.createElement("versionInfo"); + root.appendChild(versionInfo); + QDomElement fw=doc.createElement("Embedded"); + UAVObjectUtilManager* utilMngr = pm->getObject(); + fw.setAttribute("gitcommittag",utilMngr->getBoardDescriptionStruct().gitTag); + fw.setAttribute("fwtag",utilMngr->getBoardDescriptionStruct().description); + fw.setAttribute("cpuSerial",QString(utilMngr->getBoardCPUSerial().toHex())); + + versionInfo.appendChild(fw); + QDomElement gcs=doc.createElement("GCS"); + gcs.setAttribute("revision",QString::fromLatin1(Core::Constants::GCS_REVISION_STR)); + versionInfo.appendChild(gcs); // iterate over settings objects QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { foreach (UAVDataObject* obj, list) { - if (obj->isSettings()) { + if (obj->isSettings() == isSettings) { // add each object to the XML QDomElement o = doc.createElement("object"); @@ -275,10 +278,37 @@ void UAVSettingsImportExportPlugin::exportUAVSettings() } } } + + return doc.toString(4); +} + +// Slot called by the menu manager on user action +void UAVSettingsImportExportPlugin::exportUAVSettings() +{ + // ask for file name + QString fileName; + QString filters = tr("UAVSettings XML files (*.uav)"); + + fileName = QFileDialog::getSaveFileName(0, tr("Save UAV Settings File As"), "", filters); + if (fileName.isEmpty()) { + return; + } + + bool fullExport = false; + // If the filename ends with .xml, we will do a full export, otherwise, a simple export + if (fileName.endsWith(".xml")) { + fullExport = true; + } else if (!fileName.endsWith(".uav")) { + fileName.append(".uav"); + } + + // generate an XML first (used for all export formats as a formatted data source) + QString xml = createXMLDocument("UAVSettings", true, fullExport); + // save file QFile file(fileName); if (file.open(QIODevice::WriteOnly) && - (file.write(doc.toString(4).toAscii()) != -1)) { + (file.write(xml.toAscii()) != -1)) { file.close(); } else { QMessageBox::critical(0, @@ -294,7 +324,58 @@ void UAVSettingsImportExportPlugin::exportUAVSettings() msgBox.exec(); } -void UAVSettingsImportExportPlugin::shutdown() +// Slot called by the menu manager on user action +void UAVSettingsImportExportPlugin::exportUAVData() +{ + if (QMessageBox::question(0, tr("Are you sure?"), + tr("This option is only useful for passing your current " + "system data to the technical support staff. " + "Do you really want to export?"), + QMessageBox::Ok | QMessageBox::Cancel, + QMessageBox::Ok) != QMessageBox::Ok) { + return; + } + + // ask for file name + QString fileName; + QString filters = tr("UAVData XML files (*.uav)"); + + fileName = QFileDialog::getSaveFileName(0, tr("Save UAV Data File As"), "", filters); + if (fileName.isEmpty()) { + return; + } + + bool fullExport = false; + // If the filename ends with .xml, we will do a full export, otherwise, a simple export + if (fileName.endsWith(".xml")) { + fullExport = true; + } else if (!fileName.endsWith(".uav")) { + fileName.append(".uav"); + } + + // generate an XML first (used for all export formats as a formatted data source) + QString xml = createXMLDocument("UAVData", false, fullExport); + + // save file + QFile file(fileName); + if (file.open(QIODevice::WriteOnly) && + (file.write(xml.toAscii()) != -1)) { + file.close(); + } else { + QMessageBox::critical(0, + tr("UAV Data Export"), + tr("Unable to save data: ") + fileName, + QMessageBox::Ok); + return; + } + + QMessageBox msgBox; + msgBox.setText(tr("Data saved.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.exec(); +} + +void UAVSettingsImportExportPlugin::shutdown() { // Do nothing } diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h index 17804326a..ff8972227 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.h @@ -30,7 +30,7 @@ #include #include "uavobjectutil/uavobjectutilmanager.h" #include "importsummary.h" - +#include "../../../../../build/ground/openpilotgcs/gcsversioninfo.h" class UAVSettingsImportExportPlugin : public ExtensionSystem::IPlugin { Q_OBJECT @@ -43,9 +43,15 @@ public: bool initialize(const QStringList & arguments, QString * errorString); void shutdown(); +private: + QString createXMLDocument(const QString docName, + const bool isSettings, + const bool fullExport); + private slots: void importUAVSettings(); void exportUAVSettings(); + void exportUAVData(); }; diff --git a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.pro b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.pro index b653864ed..fcdc11ae6 100644 --- a/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.pro +++ b/ground/openpilotgcs/src/plugins/uavsettingsimportexport/uavsettingsimportexport.pro @@ -6,7 +6,7 @@ TARGET = UAVSettingsImportExport include(../../openpilotgcsplugin.pri) include(uavsettingsimportexport_dependencies.pri) - + HEADERS += uavsettingsimportexport.h \ importsummary.h SOURCES += uavsettingsimportexport.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pri b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pri index addc16a2a..aeb4173aa 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pri +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pri @@ -1,3 +1,3 @@ -include(uavtalk_dependencies.pri) - -LIBS *= -l$$qtLibraryTarget(UAVTalk) +include(uavtalk_dependencies.pri) + +LIBS *= -l$$qtLibraryName(UAVTalk) diff --git a/ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.cpp b/ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.cpp deleted file mode 100644 index ffbc2cb69..000000000 --- a/ground/openpilotgcs/src/plugins/uploader/devicedescriptorstruct.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "devicedescriptorstruct.h" - -deviceDescriptorStruct::deviceDescriptorStruct() -{ -} diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index 238d21bef..e2d2c0148 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -180,7 +180,7 @@ void deviceWidget::freeze() */ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc) { - if(UploaderGadgetWidget::descriptionToStructure(desc,&onBoardDescrition)) + if(UAVObjectUtilManager::descriptionToStructure(desc,&onBoardDescrition)) { myDevice->lblGitTag->setText(onBoardDescrition.gitTag); myDevice->lblBuildDate->setText(onBoardDescrition.buildDate); @@ -210,7 +210,7 @@ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc) } bool deviceWidget::populateLoadedStructuredDescription(QByteArray desc) { - if(UploaderGadgetWidget::descriptionToStructure(desc,&LoadedDescrition)) + if(UAVObjectUtilManager::descriptionToStructure(desc,&LoadedDescrition)) { myDevice->lblGitTagL->setText(LoadedDescrition.gitTag); myDevice->lblBuildDateL->setText( LoadedDescrition.buildDate); diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index e45dbad83..0db22efb0 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -110,7 +110,7 @@ void runningDeviceWidget::populate() QByteArray description = utilMngr->getBoardDescription(); deviceDescriptorStruct devDesc; - if(UploaderGadgetWidget::descriptionToStructure(description,&devDesc)) + if(UAVObjectUtilManager::descriptionToStructure(description,&devDesc)) { if(devDesc.description.startsWith("release",Qt::CaseInsensitive)) { diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.pro b/ground/openpilotgcs/src/plugins/uploader/uploader.pro index 32424509b..05742868d 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.pro +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.pro @@ -22,8 +22,7 @@ HEADERS += uploadergadget.h \ SSP/qssp.h \ SSP/qsspt.h \ SSP/common.h \ - runningdevicewidget.h \ - devicedescriptorstruct.h + runningdevicewidget.h SOURCES += uploadergadget.cpp \ uploadergadgetconfiguration.cpp \ uploadergadgetfactory.cpp \ @@ -36,8 +35,7 @@ SOURCES += uploadergadget.cpp \ SSP/port.cpp \ SSP/qssp.cpp \ SSP/qsspt.cpp \ - runningdevicewidget.cpp \ - devicedescriptorstruct.cpp + runningdevicewidget.cpp OTHER_FILES += Uploader.pluginspec FORMS += \ diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 3a5427a6c..397533c2b 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -27,44 +27,7 @@ #include "uploadergadgetwidget.h" #define DFU_DEBUG true -bool UploaderGadgetWidget::descriptionToStructure(QByteArray desc,deviceDescriptorStruct * struc) -{ - if (desc.startsWith("OpFw")) { - // This looks like a binary with a description at the end - /* - # 4 bytes: header: "OpFw" - # 4 bytes: GIT commit tag (short version of SHA1) - # 4 bytes: Unix timestamp of compile time - # 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. - # 26 bytes: commit tag if it is there, otherwise "Unreleased". Zero-padded - # ---- 40 bytes limit --- - # 20 bytes: SHA1 sum of the firmware. - # 40 bytes: free for now. - */ - // Note: the ARM binary is big-endian: - quint32 gitCommitTag = desc.at(7)&0xFF; - for (int i=1;i<4;i++) { - gitCommitTag = gitCommitTag<<8; - gitCommitTag += desc.at(7-i) & 0xFF; - } - struc->gitTag=QString::number(gitCommitTag,16); - quint32 buildDate = desc.at(11)&0xFF; - for (int i=1;i<4;i++) { - buildDate = buildDate<<8; - buildDate += desc.at(11-i) & 0xFF; - } - struc->buildDate= QDateTime::fromTime_t(buildDate).toLocalTime().toString("yyyy MMMM dd HH:mm:ss"); - QByteArray targetPlatform = desc.mid(12,2); - // TODO: check platform compatibility - QString dscText = QString(desc.mid(14,26)); - struc->boardType=(int)targetPlatform.at(0); - struc->boardRevision=(int)targetPlatform.at(1); - struc->description=dscText; - return true; - } - return false; -} UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) { m_config = new Ui_UploaderWidget(); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 81d39e44f..075dc18a0 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -68,7 +68,6 @@ public: ~UploaderGadgetWidget(); typedef enum { IAP_STATE_READY, IAP_STATE_STEP_1, IAP_STATE_STEP_2, IAP_STEP_RESET, IAP_STATE_BOOTLOADER} IAPStep; void log(QString str); - static bool descriptionToStructure(QByteArray desc,deviceDescriptorStruct * struc); public slots: void onAutopilotConnect(); diff --git a/ground/openpilotgcs/src/plugins/welcome/welcome.pri b/ground/openpilotgcs/src/plugins/welcome/welcome.pri index 2edd8a4df..41e3d85b1 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcome.pri +++ b/ground/openpilotgcs/src/plugins/welcome/welcome.pri @@ -1,3 +1,3 @@ include(welcome_dependencies.pri) -LIBS *= -l$$qtLibraryTarget(Welcome) +LIBS *= -l$$qtLibraryName(Welcome) diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index c5f67a5c8..8da08ba35 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -77,7 +77,8 @@ WelcomeModePrivate::WelcomeModePrivate() // --- WelcomeMode WelcomeMode::WelcomeMode() : - m_d(new WelcomeModePrivate) + m_d(new WelcomeModePrivate), + m_priority(Core::Constants::P_MODE_WELCOME) { m_d->m_widget = new QWidget; QVBoxLayout *l = new QVBoxLayout(m_d->m_widget); @@ -122,7 +123,7 @@ QIcon WelcomeMode::icon() const int WelcomeMode::priority() const { - return Core::Constants::P_MODE_WELCOME; + return m_priority; } QWidget* WelcomeMode::widget() diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h index 8898e7cee..33c6a1ab5 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h @@ -61,6 +61,7 @@ public: void activated(); QString contextHelpId() const { return QLatin1String("OpenPilot GCS"); } void initPlugins(); + void setPriority(int priority) { m_priority = priority; } private slots: void slotFeedback(); @@ -69,6 +70,7 @@ private slots: private: WelcomeModePrivate *m_d; + int m_priority; }; } // namespace Welcome diff --git a/package/osx/OpenPilot.dmg b/package/osx/OpenPilot.dmg index faa0ab402..c8e1056fb 100644 Binary files a/package/osx/OpenPilot.dmg and b/package/osx/OpenPilot.dmg differ diff --git a/package/osx/libraries b/package/osx/libraries index 9f78fb2f1..5c87ba15b 100755 --- a/package/osx/libraries +++ b/package/osx/libraries @@ -3,7 +3,12 @@ APP="${1?}" PLUGINS="${APP}/Contents/Plugins" OP_PLUGINS="${APP}/Contents/Plugins/OpenPilot" -QT_LIBS="QtGui QtCore QtSvg QtSql QtOpenGL QtNetwork QtXml QtDBus QtScript phonon SDL" +QT_LIBS="QtGui QtTest QtCore QtSvg QtSql QtOpenGL QtNetwork QtXml QtDBus QtScript phonon" +QT_DIR=$(otool -L "${APP}/Contents/MacOS/OpenPilot GCS" | sed -n -e 's/\/QtCore\.framework.*//p' | sed -n -E 's:^.::p') +QT_EXTRA="accessible/libqtaccessiblewidgets.dylib bearer/libqgenericbearer.dylib codecs/libqcncodecs.dylib codecs/libqjpcodecs.dylib codecs/libqkrcodecs.dylib codecs/libqtwcodecs.dylib graphicssystems/libqtracegraphicssystem.dylib imageformats/libqgif.dylib imageformats/libqico.dylib imageformats/libqjpeg.dylib imageformats/libqmng.dylib imageformats/libqtiff.dylib qmltooling/libqmldbg_inspector.dylib qmltooling/libqmldbg_tcp.dylib" + + +echo "Qt library directory is \"${QT_DIR}\"" echo "Processing Qt libraries in $1" macdeployqt "${APP}" @@ -13,7 +18,7 @@ do for g in $QT_LIBS do install_name_tool -change \ - ${g}.framework/Versions/4/${g} \ + "${QT_DIR}/${g}.framework/Versions/4/${g}" \ @executable_path/../Frameworks/${g}.framework/Versions/4/${g} \ "${f}" done @@ -24,23 +29,41 @@ done for f in ${QT_LIBS} do echo "Copying ${f}" - cp -r /Library/Frameworks/${f}.framework "${APP}/Contents/Frameworks/" + cp -r "${QT_DIR}/${f}.framework" "${APP}/Contents/Frameworks/" echo "Changing package identification of ${f}" install_name_tool -id \ - @executable_path/../Frameworks/${f}.framework/Versions/4/QtCore \ + @executable_path/../Frameworks/${f}.framework/Versions/4/${f} \ "${APP}/Contents/Frameworks/${f}.framework/Versions/4/${f}" + rm "${APP}/Contents/Frameworks/${f}.framework/${f}" + echo "Changing package linkages" for g in $QT_LIBS do install_name_tool -change \ - ${g}.framework/Versions/4/${g} \ + "${QT_DIR}/${g}.framework/Versions/4/${g}" \ @executable_path/../Frameworks/${g}.framework/Versions/4/${g} \ "${APP}/Contents/Frameworks/${f}.framework/Versions/4/${f}" done done +for f in ${QT_EXTRA} +do + echo "Changing package identification of ${f}" + install_name_tool -id \ + @executable_path/../Plugins/${f} \ + "${PLUGINS}/${f}" +done + +echo "Copying SDL" +cp -r "/Library/Frameworks/SDL.framework" "${APP}/Contents/Frameworks/" + +echo "Changing package identification of SDL" +install_name_tool -id \ + @executable_path/../Frameworks/SDL.framework/Versions/A/SDL \ + "${APP}/Contents/Frameworks/SDL.framework/Versions/A/SDL" + # deleting unnecessary files echo "Deleting unnecessary files" find "${APP}/Contents/Frameworks" -iname "current" -exec rm -rf \{\} \; diff --git a/package/osx/package b/package/osx/package index a12e7e9fe..e481353ff 100755 --- a/package/osx/package +++ b/package/osx/package @@ -20,12 +20,18 @@ device=$(hdiutil attach "${TEMP_FILE}" | \ # packaging goes here cp -r "${APP_PATH}" "/Volumes/${VOL_NAME}" -cp -r "${FW_DIR}" "/Volumes/${VOL_NAME}/firmware" +#cp -r "${FW_DIR}" "/Volumes/${VOL_NAME}/firmware" +cp "${FW_DIR}/fw_coptercontrol-${PACKAGE_LBL}.opfw" "/Volumes/${VOL_NAME}/firmware" + +cp "${ROOT_DIR}/HISTORY.txt" "/Volumes/${VOL_NAME}" "${ROOT_DIR}/package/osx/libraries" \ "/Volumes/${VOL_NAME}/OpenPilot GCS.app" || exit 1 hdiutil detach ${device} + +echo "Resizing dmg" +hdiutil resize -size 250m ${TEMP_FILE} hdiutil convert "${TEMP_FILE}" -format UDZO -o "${OUT_FILE}" # cleanup diff --git a/package/winx86/openpilotgcs.nsi b/package/winx86/openpilotgcs.nsi index 9cad5d233..9e3fa2ec3 100644 --- a/package/winx86/openpilotgcs.nsi +++ b/package/winx86/openpilotgcs.nsi @@ -33,6 +33,7 @@ ; Paths ; Tree root locations (relative to this script location) + !define PROJECT_ROOT "..\.." !define NSIS_DATA_TREE "." !define GCS_BUILD_TREE "..\..\build\ground\openpilotgcs" @@ -151,6 +152,8 @@ Section "Core files" InSecCore SectionIn RO SetOutPath "$INSTDIR\bin" File /r "${GCS_BUILD_TREE}\bin\*" + SetOutPath "$INSTDIR" + File "${PROJECT_ROOT}\HISTORY.txt" SectionEnd Section "Plugins" InSecPlugins @@ -180,12 +183,15 @@ SectionEnd Section "Localization" InSecLocalization SetOutPath "$INSTDIR\share\openpilotgcs\translations" - File /r "${GCS_BUILD_TREE}\share\openpilotgcs\translations\*.qm" +; File /r "${GCS_BUILD_TREE}\share\openpilotgcs\translations\openpilotgcs_*.qm" + File /r "${GCS_BUILD_TREE}\share\openpilotgcs\translations\qt_*.qm" SectionEnd Section "Firmware" InSecFirmware - SetOutPath "$INSTDIR\firmware\${FIRMWARE_DIR}" - File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\*" +; SetOutPath "$INSTDIR\firmware\${FIRMWARE_DIR}" +; File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\*" + SetOutPath "$INSTDIR\firmware" + File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\fw_coptercontrol-${PACKAGE_LBL}.opfw" SectionEnd Section "Shortcuts" InSecShortcuts @@ -194,6 +200,14 @@ Section "Shortcuts" InSecShortcuts CreateDirectory "$SMPROGRAMS\OpenPilot" CreateShortCut "$SMPROGRAMS\OpenPilot\OpenPilot GCS.lnk" "$INSTDIR\bin\openpilotgcs.exe" \ "" "$INSTDIR\bin\openpilotgcs.exe" 0 "" "" "${PRODUCT_NAME} ${PRODUCT_VERSION}. ${BUILD_DESCRIPTION}" + CreateShortCut "$SMPROGRAMS\OpenPilot\OpenPilot ChangeLog.lnk" "$INSTDIR\HISTORY.txt" \ + "" "$INSTDIR\bin\openpilotgcs.exe" 0 + CreateShortCut "$SMPROGRAMS\OpenPilot\OpenPilot Website.lnk" "http://www.openpilot.org" \ + "" "$INSTDIR\bin\openpilotgcs.exe" 0 + CreateShortCut "$SMPROGRAMS\OpenPilot\OpenPilot Wiki.lnk" "http://wiki.openpilot.org" \ + "" "$INSTDIR\bin\openpilotgcs.exe" 0 + CreateShortCut "$SMPROGRAMS\OpenPilot\OpenPilot Forums.lnk" "http://forums.openpilot.org" \ + "" "$INSTDIR\bin\openpilotgcs.exe" 0 CreateShortCut "$DESKTOP\OpenPilot GCS.lnk" "$INSTDIR\bin\openpilotgcs.exe" \ "" "$INSTDIR\bin\openpilotgcs.exe" 0 "" "" "${PRODUCT_NAME} ${PRODUCT_VERSION}. ${BUILD_DESCRIPTION}" CreateShortCut "$SMPROGRAMS\OpenPilot\Uninstall.lnk" "$INSTDIR\Uninstall.exe" "" "$INSTDIR\Uninstall.exe" 0 @@ -244,6 +258,7 @@ Section "un.OpenPilot GCS" UnSecProgram RMDir /r /rebootok "$INSTDIR\lib" RMDir /r /rebootok "$INSTDIR\share" RMDir /r /rebootok "$INSTDIR\firmware" + Delete /rebootok "$INSTDIR\HISTORY.txt" Delete /rebootok "$INSTDIR\Uninstall.exe" ; Remove directory diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 9176b125a..b6668350d 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -22,6 +22,8 @@ + +