diff --git a/HISTORY.txt b/HISTORY.txt index 54dc5c6d0..513b368a5 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,4 +1,9 @@ Short summary of changes. For a complete list see the git log. + +2012-10-06 +Receiver port can now be configured as PPM *and* PWM inputs. +Pin 1 is PPM, other pins are PWM inputs. + 2012-07-27 Added the ability to load stylesheets from external file according to operating system: macos.qss, linux.qss, windows.qss diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 8687defef..adb3de6ae 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -38,9 +38,15 @@ OUTDIR := $(TOP)/build/$(TARGET) DEBUG ?= NO # Include objects that are just nice information to show -DIAGNOSTICS ?= NO +STACK_DIAGNOSTICS ?= NO +MIXERSTATUS_DIAGNOSTICS ?= NO +RATEDESIRED_DIAGNOSTICS ?= NO +I2C_WDG_STATS_DIAGNOSTICS ?= NO DIAG_TASKS ?= NO +#Or just turn on all the above diagnostics. WARNING: This consumes massive amounts of memory. +ALL_DIGNOSTICS ?=NO + # Set to YES to build a FW version that will erase all flash memory ERASE_FLASH ?= NO # Set to YES to use the Servo output pins for debugging via scope or logic analyser @@ -485,11 +491,25 @@ ifeq ($(DEBUG),YES) CFLAGS += -DDEBUG endif -ifeq ($(DIAGNOSTICS),YES) -CFLAGS += -DDIAGNOSTICS +#The following Makefile command, ifneq (, $(filter) $(A), $(B) $(C)) is equivalent +# to the pseudocode `if(A== B || A==C)` +ifneq (,$(filter YES,$(STACK_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DSTACK_DIAGNOSTICS endif -ifeq ($(DIAG_TASKS),YES) +ifneq (,$(filter YES,$(MIXERSTATUS_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DMIXERSTATUS_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(RATEDESIRED_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DRATEDESIRED_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(I2C_WDG_STATS_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(DIAG_TASKS) $(ALL_DIGNOSTICS))) CFLAGS += -DDIAG_TASKS endif diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index ccbae3058..d0e8dc8cb 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -662,6 +662,33 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_PPM */ break; + case HWSETTINGS_CC_RCVRPORT_PPMPWM: + /* This is a combination of PPM and PWM inputs */ +#if defined(PIOS_INCLUDE_PPM) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PPM */ +#if defined(PIOS_INCLUDE_PWM) + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; + } +#endif /* PIOS_INCLUDE_PWM */ + break; } #if defined(PIOS_INCLUDE_GCSRCVR) @@ -683,6 +710,7 @@ void PIOS_Board_Init(void) { case HWSETTINGS_CC_RCVRPORT_DISABLED: case HWSETTINGS_CC_RCVRPORT_PWM: case HWSETTINGS_CC_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPMPWM: PIOS_Servo_Init(&pios_servo_cfg); break; case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index e59d50957..6bd471666 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -126,7 +126,7 @@ int32_t ActuatorInitialize() // Primary output of this module ActuatorCommandInitialize(); -#if defined(DIAGNOSTICS) +#if defined(MIXERSTATUS_DIAGNOSTICS) // UAVO only used for inspecting the internal status of the mixer during debug MixerStatusInitialize(); #endif @@ -212,7 +212,7 @@ static void actuatorTask(void* parameters) ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); -#if defined(DIAGNOSTICS) +#if defined(MIXERSTATUS_DIAGNOSTICS) MixerStatusGet(&mixerStatus); #endif int nMixers = 0; @@ -362,7 +362,7 @@ static void actuatorTask(void* parameters) // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); -#if defined(DIAGNOSTICS) +#if defined(MIXERSTATUS_DIAGNOSTICS) MixerStatusSet(&mixerStatus); #endif diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index fc4a5dcc5..a43162e71 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -119,7 +119,7 @@ int32_t StabilizationInitialize() // Initialize variables StabilizationSettingsInitialize(); ActuatorDesiredInitialize(); -#if defined(DIAGNOSTICS) +#if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredInitialize(); #endif @@ -172,8 +172,7 @@ static void stabilizationTask(void* parameters) StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); - -#if defined(DIAGNOSTICS) +#if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif @@ -350,7 +349,7 @@ static void stabilizationTask(void* parameters) if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); -#if defined(DIAGNOSTICS) +#if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index 715c26d72..870273a1e 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -82,7 +82,7 @@ static void objectUpdatedCb(UAVObjEvent * ev); static void updateStats(); static void updateSystemAlarms(); static void systemTask(void *parameters); -#if defined(DIAGNOSTICS) +#if defined(I2C_WDG_STATS_DIAGNOSTICS) static void updateI2Cstats(); static void updateWDGstats(); #endif @@ -118,7 +118,7 @@ int32_t SystemModInitialize(void) #if defined(DIAG_TASKS) TaskInfoInitialize(); #endif -#if defined(DIAGNOSTICS) +#if defined(I2C_WDG_STATS_DIAGNOSTICS) I2CStatsInitialize(); WatchdogStatusInitialize(); #endif @@ -168,7 +168,7 @@ static void systemTask(void *parameters) // Update the system alarms updateSystemAlarms(); -#if defined(DIAGNOSTICS) +#if defined(I2C_WDG_STATS_DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif @@ -301,7 +301,7 @@ static void objectUpdatedCb(UAVObjEvent * ev) /** * Called periodically to update the I2C statistics */ -#if defined(DIAGNOSTICS) +#if defined(I2C_WDG_STATS_DIAGNOSTICS) static void updateI2Cstats() { #if defined(PIOS_INCLUDE_I2C) diff --git a/flight/PiOS/Common/pios_bma180.c b/flight/PiOS/Common/pios_bma180.c index 2a578dd5d..65b872e2c 100644 --- a/flight/PiOS/Common/pios_bma180.c +++ b/flight/PiOS/Common/pios_bma180.c @@ -434,7 +434,7 @@ int32_t PIOS_BMA180_Test() * @brief IRQ Handler. Read data from the BMA180 FIFO and push onto a local fifo. */ int32_t bma180_irqs = 0; -void PIOS_BMA180_IRQHandler(void) +bool PIOS_BMA180_IRQHandler(void) { bma180_irqs++; @@ -470,7 +470,8 @@ void PIOS_BMA180_IRQHandler(void) data.temperature = pios_bma180_dmabuf[7]; fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data)); - + + return false; } #endif /* PIOS_INCLUDE_BMA180 */ diff --git a/flight/PiOS/Common/pios_hmc5883.c b/flight/PiOS/Common/pios_hmc5883.c index 8891709fc..14d4f96fe 100644 --- a/flight/PiOS/Common/pios_hmc5883.c +++ b/flight/PiOS/Common/pios_hmc5883.c @@ -391,9 +391,11 @@ int32_t PIOS_HMC5883_Test(void) /** * @brief IRQ Handler */ -void PIOS_HMC5883_IRQHandler(void) +bool PIOS_HMC5883_IRQHandler(void) { - pios_hmc5883_data_ready = true; + pios_hmc5883_data_ready = true + + return false; } #endif /* PIOS_INCLUDE_HMC5883 */ diff --git a/flight/PiOS/Common/pios_l3gd20.c b/flight/PiOS/Common/pios_l3gd20.c index 3151d26e2..e98a66153 100644 --- a/flight/PiOS/Common/pios_l3gd20.c +++ b/flight/PiOS/Common/pios_l3gd20.c @@ -353,7 +353,7 @@ uint8_t PIOS_L3GD20_Test(void) /** * @brief IRQ Handler. Read all the data from onboard buffer */ -void PIOS_L3GD20_IRQHandler(void) +bool PIOS_L3GD20_IRQHandler(void) { l3gd20_irq++; @@ -375,7 +375,10 @@ void PIOS_L3GD20_IRQHandler(void) memcpy((uint8_t *) &(data.gyro_x), &rec[1], 6); data.temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); - xQueueSend(dev->queue, (void *) &data, 0); + portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); + + return xHigherPriorityTaskWoken == pdTRUE; } #endif /* L3GD20 */ diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index b77a4f6df..c5e0e069f 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -400,21 +400,21 @@ uint32_t mpu6000_interval_us; uint32_t mpu6000_time_us; uint32_t mpu6000_transfer_size; -void PIOS_MPU6000_IRQHandler(void) +bool PIOS_MPU6000_IRQHandler(void) { static uint32_t timeval; mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if(!mpu6000_configured) - return; + return false; mpu6000_count = PIOS_MPU6000_FifoDepth(); if(mpu6000_count < sizeof(struct pios_mpu6000_data)) - return; + return false; if(PIOS_MPU6000_ClaimBus() != 0) - return; + return false; uint8_t mpu6000_send_buf[1+sizeof(struct pios_mpu6000_data)] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t mpu6000_rec_buf[1+sizeof(struct pios_mpu6000_data)]; @@ -422,7 +422,7 @@ void PIOS_MPU6000_IRQHandler(void) if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBus(); mpu6000_fails++; - return; + return false; } PIOS_MPU6000_ReleaseBus(); @@ -433,12 +433,12 @@ void PIOS_MPU6000_IRQHandler(void) if (mpu6000_count >= (sizeof(data) * 2)) { mpu6000_fifo_backup++; if(PIOS_MPU6000_ClaimBus() != 0) - return; + return false; if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBus(); mpu6000_fails++; - return; + return false; } PIOS_MPU6000_ReleaseBus(); @@ -458,12 +458,15 @@ void PIOS_MPU6000_IRQHandler(void) data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; data.gyro_z = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; #endif - - xQueueSend(dev->queue, (void *) &data, 0); + + portBASE_TYPE xHigherPriorityTaskWoken; + xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken); mpu6000_irq++; mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); + + return xHigherPriorityTaskWoken == pdTRUE; } #endif diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 1f65748ee..74230c7a1 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -172,7 +172,7 @@ uint32_t random32 = 0x459ab8d8; /* Local function forwared declarations */ static void PIOS_RFM22B_Supervisor(uint32_t ppm_id); static void rfm22_processInt(void); -static void PIOS_RFM22_EXT_Int(void); +static bool PIOS_RFM22_EXT_Int(void); static void rfm22_setTxMode(uint8_t mode); // SPI read/write functions @@ -751,12 +751,13 @@ uint8_t rfm22_read(uint8_t addr) // external interrupt -static void PIOS_RFM22_EXT_Int(void) +static bool PIOS_RFM22_EXT_Int(void) { rfm22_setDebug("Ext Int"); if (!exec_using_spi) rfm22_processInt(); rfm22_setDebug("Ext Done"); + return false; } void rfm22_disableExtInt(void) diff --git a/flight/PiOS/STM32F10x/pios_dsm.c b/flight/PiOS/STM32F10x/pios_dsm.c index cb838b56b..b35560d5d 100644 --- a/flight/PiOS/STM32F10x/pios_dsm.c +++ b/flight/PiOS/STM32F10x/pios_dsm.c @@ -128,7 +128,7 @@ static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind) GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */ - PIOS_DELAY_WaitmS(60); + PIOS_DELAY_WaitmS(20); for (int i = 0; i < bind ; i++) { /* RX line, drive low for 120us */ diff --git a/flight/PiOS/STM32F10x/pios_exti.c b/flight/PiOS/STM32F10x/pios_exti.c index 65a651f6f..9b8221bbe 100644 --- a/flight/PiOS/STM32F10x/pios_exti.c +++ b/flight/PiOS/STM32F10x/pios_exti.c @@ -149,7 +149,7 @@ out_fail: return -1; } -static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) +static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; @@ -158,69 +158,133 @@ static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || cfg_index == PIOS_EXTI_INVALID) { /* Unconfigured interrupt just fired! */ - return; + return false; } struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - cfg->vector(); + return cfg->vector(); } -/* Bind Interrupt Handlers */ - -#define PIOS_EXTI_HANDLE_LINE(line) \ +#ifdef PIOS_INCLUDE_FREERTOS +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } +#else +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ EXTI_ClearITPendingBit(EXTI_Line##line); \ PIOS_EXTI_generic_irq_handler(line); \ } +#endif + +/* Bind Interrupt Handlers */ static void PIOS_EXTI_0_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(0); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); static void PIOS_EXTI_1_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(1); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); static void PIOS_EXTI_2_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(2); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); static void PIOS_EXTI_3_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(3); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); static void PIOS_EXTI_4_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(4); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); static void PIOS_EXTI_9_5_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(5); - PIOS_EXTI_HANDLE_LINE(6); - PIOS_EXTI_HANDLE_LINE(7); - PIOS_EXTI_HANDLE_LINE(8); - PIOS_EXTI_HANDLE_LINE(9); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); static void PIOS_EXTI_15_10_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(10); - PIOS_EXTI_HANDLE_LINE(11); - PIOS_EXTI_HANDLE_LINE(12); - PIOS_EXTI_HANDLE_LINE(13); - PIOS_EXTI_HANDLE_LINE(14); - PIOS_EXTI_HANDLE_LINE(15); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; // dummy variable +#endif + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); diff --git a/flight/PiOS/STM32F4xx/pios_exti.c b/flight/PiOS/STM32F4xx/pios_exti.c index 9f9567d51..45efb2ae5 100644 --- a/flight/PiOS/STM32F4xx/pios_exti.c +++ b/flight/PiOS/STM32F4xx/pios_exti.c @@ -149,7 +149,7 @@ out_fail: return -1; } -static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) +static bool PIOS_EXTI_generic_irq_handler(uint8_t line_index) { uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; @@ -158,69 +158,133 @@ static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || cfg_index == PIOS_EXTI_INVALID) { /* Unconfigured interrupt just fired! */ - return; + return false; } struct pios_exti_cfg * cfg = &__start__exti + cfg_index; - cfg->vector(); + return cfg->vector(); } /* Bind Interrupt Handlers */ -#define PIOS_EXTI_HANDLE_LINE(line) \ +#ifdef PIOS_INCLUDE_FREERTOS +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + woken = PIOS_EXTI_generic_irq_handler(line) ? pdTRUE : woken; \ + } +#else +#define PIOS_EXTI_HANDLE_LINE(line, woken) \ if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ EXTI_ClearITPendingBit(EXTI_Line##line); \ PIOS_EXTI_generic_irq_handler(line); \ } +#endif static void PIOS_EXTI_0_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(0); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(0, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); static void PIOS_EXTI_1_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(1); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(1, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); static void PIOS_EXTI_2_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(2); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(2, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); static void PIOS_EXTI_3_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(3); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(3, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); static void PIOS_EXTI_4_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(4); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(4, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); static void PIOS_EXTI_9_5_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(5); - PIOS_EXTI_HANDLE_LINE(6); - PIOS_EXTI_HANDLE_LINE(7); - PIOS_EXTI_HANDLE_LINE(8); - PIOS_EXTI_HANDLE_LINE(9); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(5, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(6, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(7, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(8, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(9, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); static void PIOS_EXTI_15_10_irq_handler (void) { - PIOS_EXTI_HANDLE_LINE(10); - PIOS_EXTI_HANDLE_LINE(11); - PIOS_EXTI_HANDLE_LINE(12); - PIOS_EXTI_HANDLE_LINE(13); - PIOS_EXTI_HANDLE_LINE(14); - PIOS_EXTI_HANDLE_LINE(15); +#ifdef PIOS_INCLUDE_FREERTOS + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#else + bool xHigherPriorityTaskWoken; +#endif + PIOS_EXTI_HANDLE_LINE(10, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(11, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(12, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(13, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(14, xHigherPriorityTaskWoken); + PIOS_EXTI_HANDLE_LINE(15, xHigherPriorityTaskWoken); +#ifdef PIOS_INCLUDE_FREERTOS + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif } void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); diff --git a/flight/PiOS/inc/pios_bma180.h b/flight/PiOS/inc/pios_bma180.h index ef9627f00..51e65a6ec 100644 --- a/flight/PiOS/inc/pios_bma180.h +++ b/flight/PiOS/inc/pios_bma180.h @@ -103,7 +103,7 @@ extern float PIOS_BMA180_GetScale(); extern int32_t PIOS_BMA180_ReadFifo(struct pios_bma180_data * buffer); extern int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data); extern int32_t PIOS_BMA180_Test(); -extern void PIOS_BMA180_IRQHandler(); +extern bool PIOS_BMA180_IRQHandler(); #endif /* PIOS_BMA180_H */ diff --git a/flight/PiOS/inc/pios_exti.h b/flight/PiOS/inc/pios_exti.h index 73a3bb556..4b1203aa2 100644 --- a/flight/PiOS/inc/pios_exti.h +++ b/flight/PiOS/inc/pios_exti.h @@ -36,7 +36,7 @@ #include struct pios_exti_cfg { - void (* vector)(void); + bool (* vector)(void); uint32_t line; /* use EXTI_LineN macros */ struct stm32_gpio pin; struct stm32_irq irq; diff --git a/flight/PiOS/inc/pios_hmc5883.h b/flight/PiOS/inc/pios_hmc5883.h index 037dca788..d9c362e9d 100644 --- a/flight/PiOS/inc/pios_hmc5883.h +++ b/flight/PiOS/inc/pios_hmc5883.h @@ -107,7 +107,7 @@ extern bool PIOS_HMC5883_NewDataAvailable(void); extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); extern int32_t PIOS_HMC5883_Test(void); -extern void PIOS_HMC5883_IRQHandler(); +bool void PIOS_HMC5883_IRQHandler(); #endif /* PIOS_HMC5883_H */ /** diff --git a/flight/PiOS/inc/pios_l3gd20.h b/flight/PiOS/inc/pios_l3gd20.h index 83b2070b6..3a2e896fd 100644 --- a/flight/PiOS/inc/pios_l3gd20.h +++ b/flight/PiOS/inc/pios_l3gd20.h @@ -141,7 +141,7 @@ extern int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range); extern float PIOS_L3GD20_GetScale(); extern int32_t PIOS_L3GD20_ReadID(); extern uint8_t PIOS_L3GD20_Test(); -extern void PIOS_L3GD20_IRQHandler(); +bool void PIOS_L3GD20_IRQHandler(); #endif /* PIOS_L3GD20_H */ diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h index 2a301bc4c..2c9f0f8bb 100644 --- a/flight/PiOS/inc/pios_mpu6000.h +++ b/flight/PiOS/inc/pios_mpu6000.h @@ -156,7 +156,7 @@ extern int32_t PIOS_MPU6000_ReadID(); extern uint8_t PIOS_MPU6000_Test(); extern float PIOS_MPU6000_GetScale(); extern float PIOS_MPU6000_GetAccelScale(); -extern void PIOS_MPU6000_IRQHandler(void); +extern bool PIOS_MPU6000_IRQHandler(void); #endif /* PIOS_MPU6000_H */ diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 63f2e7907..2aa57825a 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -38,7 +38,14 @@ OUTDIR := $(TOP)/build/$(TARGET) DEBUG ?= NO # Include objects that are just nice information to show -DIAGNOSTICS ?= NO +STACK_DIAGNOSTICS ?= NO +MIXERSTATUS_DIAGNOSTICS ?= NO +RATEDESIRED_DIAGNOSTICS ?= NO +I2C_WDG_STATS_DIAGNOSTICS ?= NO +DIAG_TASKS ?= NO + +#Or just turn on all the above diagnostics. WARNING: This consumes massive amounts of memory. +ALL_DIGNOSTICS ?=NO # Set to YES to build a FW version that will erase all flash memory ERASE_FLASH ?= NO @@ -378,8 +385,24 @@ ifeq ($(DEBUG),YES) CFLAGS = -DDEBUG endif -ifeq ($(DIAGNOSTICS),YES) -CFLAGS = -DDIAGNOSTICS +ifneq (,$(filter YES,$(STACK_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DSTACK_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(MIXERSTATUS_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DMIXERSTATUS_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(RATEDESIRED_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DRATEDESIRED_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(I2C_WDG_STATS_DIAGNOSTICS) $(ALL_DIGNOSTICS))) +CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS +endif + +ifneq (,$(filter YES,$(DIAG_TASKS) $(ALL_DIGNOSTICS))) +CFLAGS += -DDIAG_TASKS endif CFLAGS += -g$(DEBUGF) diff --git a/flight/PipXtreme/System/inc/FreeRTOSConfig.h b/flight/PipXtreme/System/inc/FreeRTOSConfig.h index 994956008..0c515b7f7 100755 --- a/flight/PipXtreme/System/inc/FreeRTOSConfig.h +++ b/flight/PipXtreme/System/inc/FreeRTOSConfig.h @@ -76,7 +76,7 @@ NVIC value of 255. */ #endif /* Enable run time stats collection */ -#if defined(DIAGNOSTICS) +#if defined(STACK_DIAGNOSTICS) #define configCHECK_FOR_STACK_OVERFLOW 2 #define configGENERATE_RUN_TIME_STATS 1 diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index ce8a45b6d..ff76ddbfa 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -255,7 +255,10 @@ endif # common architecture-specific flags from the device-specific library makefile CFLAGS += $(ARCHFLAGS) -CFLAGS += -DDIAGNOSTICS +CFLAGS += -DSTACK_DIAGNOSTICS +CFLAGS += -DMIXERSTATUS_DIAGNOSTICS +CFLAGS += -DRATEDESIRED_DIAGNOSTICS +CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS CFLAGS += -DDIAG_TASKS # This is not the best place for these. Really should abstract out diff --git a/flight/SimPosix/Makefile b/flight/SimPosix/Makefile index 622d7f53c..9e24125ee 100644 --- a/flight/SimPosix/Makefile +++ b/flight/SimPosix/Makefile @@ -256,7 +256,11 @@ endif CFLAGS += $(ARCHFLAGS) CFLAGS += $(UAVOBJDEFINE) -CFLAGS += -DDIAGNOSTICS + +CFLAGS += -DSTACK_DIAGNOSTICS +CFLAGS += -DMIXERSTATUS_DIAGNOSTICS +CFLAGS += -DRATEDESIRED_DIAGNOSTICS +CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS CFLAGS += -DDIAG_TASKS # This is not the best place for these. Really should abstract out diff --git a/flight/board_hw_defs/coptercontrol/board_hw_defs.c b/flight/board_hw_defs/coptercontrol/board_hw_defs.c index cc6ec4552..403657199 100644 --- a/flight/board_hw_defs/coptercontrol/board_hw_defs.c +++ b/flight/board_hw_defs/coptercontrol/board_hw_defs.c @@ -1134,6 +1134,19 @@ const struct pios_pwm_cfg pios_pwm_cfg = { .channels = pios_tim_rcvrport_all_channels, .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; + +const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Leave the first channel for PPM use and use the rest for PWM */ + .channels = &pios_tim_rcvrport_all_channels[1], + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1, +}; + #endif #if defined(PIOS_INCLUDE_I2C) diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index cab264e14..006e61d50 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -8,6 +8,8 @@ false en_AU true + 700 + 800 default false @@ -1683,7 +1685,7 @@ false mapquad.png true - false + true @@ -1703,7 +1705,7 @@ false airplanepip.png true - false + true @@ -1723,7 +1725,7 @@ false mapquad.png true - false + true @@ -2636,7 +2638,7 @@ uavGadget 2 - @Variant(AAAACQAAAAIAAAACAAAClQAAAAIAAAB3) + @Variant(AAAACQAAAAIAAAACAAABhAAAAAIAAAGE) splitter 1 @@ -2669,8 +2671,20 @@ - EmptyGadget - uavGadget + + PfdQmlGadget + + NoTerrain + + uavGadget + + + MagicWaypointGadget + uavGadget + + 1 + @Variant(AAAACQAAAAIAAAACAAAB7wAAAAIAAAGU) + splitter GCSControlGadget @@ -2680,7 +2694,7 @@ uavGadget 1 - @Variant(AAAACQAAAAIAAAACAAAB6AAAAAIAAADC) + @Variant(AAAACQAAAAIAAAACAAADhAAAAAIAAAIX) splitter 2 @@ -2739,7 +2753,7 @@ false - :\core\images\ah.png + :/core/images/ah.png :/core/images/openpilot_logo_64.png :/core/images/config.png :/core/images/cog.png diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index 960c10355..db715a74b 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -6,7 +6,7 @@ Notify Plugin settings true - 600 + 700 800 false Wide configuration @@ -1679,7 +1679,7 @@ false mapquad.png true - false + true @@ -1698,7 +1698,7 @@ false airplanepip.png true - false + true @@ -1717,7 +1717,7 @@ false mapquad.png true - false + true @@ -2636,7 +2636,7 @@ uavGadget 2 - @Variant(AAAACQAAAAIAAAACAAAClQAAAAIAAAB3) + @Variant(AAAACQAAAAIAAAACAAABhAAAAAIAAAGE) splitter 1 @@ -2669,8 +2669,20 @@ - EmptyGadget - uavGadget + + PfdQmlGadget + + NoTerrain + + uavGadget + + + MagicWaypointGadget + uavGadget + + 1 + @Variant(AAAACQAAAAIAAAACAAAB7wAAAAIAAAGU) + splitter GCSControlGadget @@ -2680,7 +2692,7 @@ uavGadget 1 - @Variant(AAAACQAAAAIAAAACAAAB6AAAAAIAAADC) + @Variant(AAAACQAAAAIAAAACAAADhAAAAAIAAAIX) splitter 2 @@ -2739,7 +2751,7 @@ false - :\core\images\ah.png + :/core/images/ah.png :/core/images/openpilot_logo_64.png :/core/images/config.png :/core/images/cog.png diff --git a/ground/openpilotgcs/share/share.pro b/ground/openpilotgcs/share/share.pro index 31b545e71..3c2a517b8 100644 --- a/ground/openpilotgcs/share/share.pro +++ b/ground/openpilotgcs/share/share.pro @@ -6,13 +6,14 @@ SUBDIRS = openpilotgcs/translations DATACOLLECTIONS = dials models pfd sounds diagrams mapicons stylesheets default_configurations equals(copydata, 1) { - for(dir, DATACOLLECTIONS) { - exists($$GCS_SOURCE_TREE/share/openpilotgcs/$$dir) { - macx:data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/\") $$addNewline() - !macx:data_copy.commands += $(MKDIR) $$targetPath(\"$$GCS_DATA_PATH/$$dir\") $$addNewline() - !macx:data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/\") $$addNewline() - } - } + for(dir, DATACOLLECTIONS) { + exists($$GCS_SOURCE_TREE/share/openpilotgcs/$$dir) { + macx:data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/\") $$addNewline() + win32:data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/$$dir\") $$addNewline() + unix:data_copy.commands += $(MKDIR) $$targetPath(\"$$GCS_DATA_PATH/$$dir\") $$addNewline() + unix:data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/\") $$addNewline() + } + } data_copy.target = FORCE QMAKE_EXTRA_TARGETS += data_copy diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp index 3b858d82f..86660fd3e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp @@ -43,8 +43,6 @@ namespace core { LanguageStr=LanguageType().toShortString(Language); Cache::Instance(); -// OPMaps::MemoryCache(); - } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h index 66581c694..d3e09b868 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h @@ -3,25 +3,25 @@ * * @file lks94projection.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. -* @brief +* @brief * @see The GNU Public License (GPL) Version 3 * @defgroup OPMapWidget * @{ -* +* *****************************************************************************/ -/* -* 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 +/* +* 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 +* +* 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., +* +* 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 LKS94PROJECTION_H @@ -30,7 +30,7 @@ #include "cmath" #include "../pureprojection.h" - + namespace projections { class LKS94Projection:public internals::PureProjection { diff --git a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp index 2293c49ba..cbaa87453 100644 --- a/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp +++ b/ground/openpilotgcs/src/libs/sdlgamepad/sdlgamepad.cpp @@ -93,8 +93,18 @@ bool SDLGamepad::setGamepad(qint16 index) { buttons = SDL_JoystickNumButtons(gamepad); axes = SDL_JoystickNumAxes(gamepad); - this->index = index; - return true; + + if (axes >= 4) { + this->index = index; + return true; + } + else + { + buttons = -1; + axes = -1; + this->index = -1; + qCritical("Gamepad has less than 4 axes"); + } } else { diff --git a/ground/openpilotgcs/src/libs/utils/mylistwidget.h b/ground/openpilotgcs/src/libs/utils/mylistwidget.h index 40059927e..176063681 100644 --- a/ground/openpilotgcs/src/libs/utils/mylistwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mylistwidget.h @@ -42,7 +42,7 @@ class QTCREATOR_UTILS_EXPORT MyListWidget : public QListWidget { Q_OBJECT public: - MyListWidget(QWidget *parent) : QListWidget(parent), m_iconAbove(false) { } + MyListWidget(QWidget *parent) : QListWidget(parent), m_iconAbove(false) {} void setIconAbove(bool iconAbove) { m_iconAbove = iconAbove; } protected: QStyleOptionViewItem viewOptions() const; diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index 3db71a316..639052e57 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -48,20 +48,24 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool toplevelLayout->addWidget(m_stackWidget); m_listWidget->setFlow(QListView::TopToBottom); m_listWidget->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); + m_listWidget->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); } else { toplevelLayout = new QVBoxLayout; toplevelLayout->addWidget(m_stackWidget); toplevelLayout->addWidget(m_listWidget); m_listWidget->setFlow(QListView::LeftToRight); m_listWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + m_listWidget->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); } - if (m_iconAbove && m_vertical) + + 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_listWidget->setSpacing(0); m_stackWidget->setContentsMargins(0, 0, 0, 0); setLayout(toplevelLayout); diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index f32524ce5..5faad8222 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -52,6 +52,7 @@ public: void insertCornerWidget(int index, QWidget *widget); int cornerWidgetCount() { return m_cornerWidgetCount; } QWidget * currentWidget(){return m_stackWidget->currentWidget();} + QWidget * getWidget(int index) {return m_stackWidget->widget(index);} signals: void currentAboutToShow(int index,bool * proceed); diff --git a/ground/openpilotgcs/src/plugins/config/Config.pluginspec b/ground/openpilotgcs/src/plugins/config/Config.pluginspec index 1ba1c8f76..f9ce7a7e5 100644 --- a/ground/openpilotgcs/src/plugins/config/Config.pluginspec +++ b/ground/openpilotgcs/src/plugins/config/Config.pluginspec @@ -1,4 +1,4 @@ - + The OpenPilot Project (C) 2010 OpenPilot Project GNU Public License (GPL) Version 3 diff --git a/ground/openpilotgcs/src/plugins/config/airframe.ui b/ground/openpilotgcs/src/plugins/config/airframe.ui index 4b1b44d07..dda3534d4 100755 --- a/ground/openpilotgcs/src/plugins/config/airframe.ui +++ b/ground/openpilotgcs/src/plugins/config/airframe.ui @@ -6,8 +6,8 @@ 0 0 - 965 - 637 + 880 + 608 @@ -128,8 +128,8 @@ 0 0 - 935 - 537 + 850 + 508 @@ -1092,7 +1092,7 @@ margin:1px; Typical value is 50% for + or X configuration on quads. - -100 + 0 100 @@ -1450,7 +1450,7 @@ font: bold 12px; margin:1px; - Multirotor Yaw Direction + Multirotor Motor Direction Qt::AlignCenter @@ -1506,9 +1506,9 @@ margin:1px; - + - Reverse Yaw Mix + Reverse all motors @@ -1544,487 +1544,506 @@ margin:1px; false - + - - - + + + true + + + + + 0 + 0 + 800 + 386 + + + - - - - 0 - 0 - - - - - 75 - true - - - - Vehicle type: - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - 75 - true - - - - Channel Assignment - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - 0 - 0 - - - - - 0 - 100 - - - - Output channel asignmets - - - - - - - 77 - 0 - - - - Engine - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Select output channel for the engine - - - - - - - - 60 - 0 - - - - Aileron 1 - - - - - - - Select output channel for the first aileron (or elevon) - - - - - - - false - - - - 60 - 0 - - - - Aileron 2 - - - - - - - false - - - Select output channel for the second aileron (or elevon) - - - - - - - - 0 - 0 - - - - Motor - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Select output channel for the first motor - - - - - - - false - - - - 47 - 0 - - - - Motor 2 - - - - - - - false - - - Select output channel for a second motor - - - - - - - Front Steering - - - - - - - Select output channel for the first steering actuator - - - - - - - Rear Steering - - - - - - - Select output channel for a second steering actuator - - - - - - - - - - true - - - - 0 - 0 - - - - Differential Steering Mix - - - - - - - - + + + + + + + + 0 + 0 + + + + + 75 + true + + + + Vehicle type: + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 75 + true + + + + Channel Assignment + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 100 + + + + Output channel asignmets + + + + - 65 + 77 0 - Left % + Engine + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - 100 - - - 50 - - - Qt::Vertical + + + + Select output channel for the engine - - - - 50 - - - - - - - - - + + - 50 + 60 0 - Right % + Aileron 1 - - - - 100 - - - 50 - - - Qt::Vertical + + + + Select output channel for the first aileron (or elevon) - - + + + + false + + + + 60 + 0 + + - 50 + Aileron 2 + + + + + + + false + + + Select output channel for the second aileron (or elevon) + + + + + + + + 0 + 0 + + + + Motor + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Select output channel for the first motor + + + + + + + false + + + + 47 + 0 + + + + Motor 2 + + + + + + + false + + + Select output channel for a second motor + + + + + + + Front Steering + + + + + + + Select output channel for the first steering actuator + + + + + + + Rear Steering + + + + + + + Select output channel for a second steering actuator - - - - - - - - - - - 0 - 100 - - - - Front throttle curve - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 500 - 500 - - - - - 10 - 10 - - - - - 300 - 350 - - - - - - - - - - - - 0 - 0 - - - - Rear throttle curve - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 500 - 500 - - - - - 10 - 10 - - - - - 300 - 350 - - - - - - + + + + + + true + + + + 0 + 0 + + + + Differential Steering Mix + + + + + + + + + + + 65 + 0 + + + + Left % + + + + + + + 100 + + + 50 + + + Qt::Vertical + + + + + + + 50 + + + + + + + + + + + + 50 + 0 + + + + Right % + + + + + + + 100 + + + 50 + + + Qt::Vertical + + + + + + + 50 + + + + + + + + + + + + + + + 0 + 100 + + + + Front throttle curve + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 500 + 500 + + + + + 10 + 10 + + + + + 300 + 350 + + + + + + + + + + + + 0 + 0 + + + + Rear throttle curve + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 500 + 500 + + + + + 10 + 10 + + + + + 300 + 350 + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 75 + true + + + + Mixer OK + + + + + + - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 75 - true - - - - Mixer OK - - - - - - + + @@ -2813,8 +2832,8 @@ margin:1px; 0 0 - 935 - 537 + 287 + 326 @@ -3203,6 +3222,7 @@ p, li { white-space: pre-wrap; } <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 REQUIRES CAUTION</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;"><br /></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;"><br /></span></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> @@ -3255,7 +3275,7 @@ p, li { white-space: pre-wrap; } - + :/core/images/helpicon.svg:/core/images/helpicon.svg @@ -3308,7 +3328,7 @@ p, li { white-space: pre-wrap; } - + diff --git a/ground/openpilotgcs/src/plugins/config/autotune.ui b/ground/openpilotgcs/src/plugins/config/autotune.ui index 9625c1060..647e7b166 100644 --- a/ground/openpilotgcs/src/plugins/config/autotune.ui +++ b/ground/openpilotgcs/src/plugins/config/autotune.ui @@ -21,7 +21,10 @@ - -1 + 6 + + + 12 @@ -33,42 +36,10 @@ Pre-Autotune - + 12 - - 12 - - - 12 - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - + QFrame::StyledPanel @@ -80,28 +51,55 @@ <!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;"> -<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-size:20pt; font-weight:600; color:#ff0000;">WARNING:</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;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">This is an experimental plugin for the GCS that is going to make your aircraft shake etc so test with lots of space and be <span style=" font-weight:600;">very very wary</span> for it creating bad tuning values.  Basically there is no reason to think this will work at all.<br /><br />To use autotuning, here are the steps:</p> -<ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style=" margin-top:12px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Go to the UAVOBrowser and under HwSettings.OptionalModules enable Autotune.  Click send then save.  Power cycle your board (disconnect battery AND usb).<br /></li> -<li style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">In Input configuration set one of your flight modes to &quot;Autotune&quot;<br /></li> -<li style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Take off, flip to autotune, keep it in the air while it's shaking<br /></li> -<li style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Land and disarm.  (note - you <span style=" font-weight:600;">MUST</span> stay in autotune mode through this point, leaving autotune before disarming aborts the process)<br /></li> -<li style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">We'd recommend checking your stabilization settings before trying them out. <br /></li> -<li style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Test fly then new settings</li> -<li style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">If you're ready to proceed, click the &quot;Enable Autotune Module&quot; checkbox below this text, and go to the next tab.</li></ul></body></html> +</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> +<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:'Lucida Grande'; font-size:20pt; font-weight:600; color:#ff0000;">WARNING:</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:'Lucida Grande'; font-size:13pt;"><br /></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:'Lucida Grande'; font-size:13pt;"><br /></span></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:'Lucida Grande'; font-size:13pt;">This is an experimental plugin for the GCS that is going to make your aircraft shake, etc, so test with lots of space and be </span><span style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:600;">very very wary</span><span style=" font-family:'Lucida Grande'; font-size:13pt;"> for it creating bad tuning values.  Basically there is no reason to think this will work at all.<br /><br />To use autotuning, here are the steps:<br /></span></p> +<ul style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">On the <span style=" font-style:italic;">Input configuration</span> tab, <span style=" font-style:italic;">Flight Mode Switch Settings</span>, set one of your flight modes to &quot;Autotune&quot;.<br /></li> +<li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Take off, change flight mode to autotune, keep it in the air while it's shaking.<br /></li> +<li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Land and disarm.  (note - you <span style=" font-weight:600;">MUST</span> stay in autotune mode through this point, leaving autotune before disarming aborts the process)<br /></li> +<li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">We'd recommend checking your stabilization settings before trying them out (ie: compare to what you currently use, if they are VASTLY different, probably a good indication bad things will happen).<br /></li> +<li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Test fly the new settings.</li> +<li style=" font-family:'Lucida Grande'; font-size:13pt;" style=" margin-top:0px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">If you're ready to proceed, click the <span style=" font-style:italic;">Enable Autotune Module</span> checkbox above this text, click <span style=" font-style:italic;">save</span> and go to the next tab.</li></ul></body></html> - - - - Enable Autotune Module - - - true + + + + Module Control + + + + + Enable Autotune Module + + + true + + + + + + + Qt::Horizontal + + + + 454 + 20 + + + + + + horizontalSpacer_5 + enableAutoTune + horizontalSpacer_4 + enableAutoTune + horizontalSpacer_5 @@ -194,8 +192,8 @@ p, li { white-space: pre-wrap; } 0 0 - 709 - 588 + 711 + 596 @@ -690,8 +688,6 @@ Useful if you have accidentally changed some settings. - - - + diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui index 14f2aa74a..b2b756ca4 100755 --- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -6,8 +6,8 @@ 0 0 - 942 - 692 + 786 + 566 @@ -26,9 +26,6 @@ Form - - 12 - @@ -67,8 +64,8 @@ 0 0 - 912 - 598 + 741 + 559 @@ -407,7 +404,7 @@ margin:1px; - -1 + 6 @@ -1032,24 +1029,24 @@ value. - - + + 4 - + Qt::Horizontal - 321 - 16 + 288 + 18 - + @@ -1098,7 +1095,7 @@ value. - + @@ -1129,7 +1126,7 @@ Apply or Save button afterwards. - + @@ -1162,7 +1159,7 @@ Apply or Save button afterwards. - + @@ -1189,7 +1186,7 @@ Apply or Save button afterwards. - + diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index 79df8ece7..a390eca95 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -49,7 +49,7 @@ const QString ConfigMultiRotorWidget::CHANNELBOXNAME = QString("multiMotorChanne /** Constructor */ -ConfigMultiRotorWidget::ConfigMultiRotorWidget(Ui_AircraftWidget *aircraft, QWidget *parent) : VehicleConfig(parent) +ConfigMultiRotorWidget::ConfigMultiRotorWidget(Ui_AircraftWidget *aircraft, QWidget *parent) : VehicleConfig(parent), invertMotors(1) { m_aircraft = aircraft; } @@ -87,20 +87,18 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) if (frameType == "Tri" || frameType == "Tricopter Y") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Tricopter Y")); - quad->setElementId("tri"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 3, true); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(100); - m_aircraft->mrYawMixLevel->setValue(50); + setYawMixLevel(50); m_aircraft->triYawChannelBox->setEnabled(true); } else if (frameType == "QuadX" || frameType == "Quad X") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Quad X")); - quad->setElementId("quad-x"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 4, true); @@ -108,104 +106,173 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) // init mixer levels m_aircraft->mrRollMixLevel->setValue(50); m_aircraft->mrPitchMixLevel->setValue(50); - m_aircraft->mrYawMixLevel->setValue(50); + setYawMixLevel(50); } else if (frameType == "QuadP" || frameType == "Quad +") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Quad +")); - quad->setElementId("quad-plus"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 4, true); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(100); - m_aircraft->mrYawMixLevel->setValue(50); + setYawMixLevel(50); } else if (frameType == "Hexa" || frameType == "Hexacopter") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Hexacopter")); - quad->setElementId("quad-hexa"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 6, true); m_aircraft->mrRollMixLevel->setValue(50); m_aircraft->mrPitchMixLevel->setValue(33); - m_aircraft->mrYawMixLevel->setValue(33); + setYawMixLevel(33); } else if (frameType == "HexaX" || frameType == "Hexacopter X" ) { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Hexacopter X")); - quad->setElementId("quad-hexa-H"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 6, true); m_aircraft->mrRollMixLevel->setValue(33); m_aircraft->mrPitchMixLevel->setValue(50); - m_aircraft->mrYawMixLevel->setValue(33); + setYawMixLevel(33); } else if (frameType == "HexaCoax" || frameType == "Hexacopter Y6") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Hexacopter Y6")); - quad->setElementId("hexa-coax"); //Enable all necessary motor channel boxes... enableComboBoxes(uiowner, CHANNELBOXNAME, 6, true); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(50); - m_aircraft->mrYawMixLevel->setValue(66); + setYawMixLevel(66); } else if (frameType == "Octo" || frameType == "Octocopter") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter")); - quad->setElementId("quad-octo"); //Enable all necessary motor channel boxes enableComboBoxes(uiowner, CHANNELBOXNAME, 8, true); m_aircraft->mrRollMixLevel->setValue(33); m_aircraft->mrPitchMixLevel->setValue(33); - m_aircraft->mrYawMixLevel->setValue(25); + setYawMixLevel(25); } else if (frameType == "OctoV" || frameType == "Octocopter V") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter V")); - quad->setElementId("quad-octo-v"); //Enable all necessary motor channel boxes enableComboBoxes(uiowner, CHANNELBOXNAME, 8, true); m_aircraft->mrRollMixLevel->setValue(25); m_aircraft->mrPitchMixLevel->setValue(25); - m_aircraft->mrYawMixLevel->setValue(25); + setYawMixLevel(25); } else if (frameType == "OctoCoaxP" || frameType == "Octo Coax +") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octo Coax +")); - quad->setElementId("octo-coax-P"); //Enable all necessary motor channel boxes enableComboBoxes(uiowner, CHANNELBOXNAME, 8, true); m_aircraft->mrRollMixLevel->setValue(100); m_aircraft->mrPitchMixLevel->setValue(100); - m_aircraft->mrYawMixLevel->setValue(50); + setYawMixLevel(50); } else if (frameType == "OctoCoaxX" || frameType == "Octo Coax X") { setComboCurrentIndex( m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octo Coax X")); - quad->setElementId("octo-coax-X"); + //Enable all necessary motor channel boxes enableComboBoxes(uiowner, CHANNELBOXNAME, 8, true); m_aircraft->mrRollMixLevel->setValue(50); m_aircraft->mrPitchMixLevel->setValue(50); - m_aircraft->mrYawMixLevel->setValue(50); + setYawMixLevel(50); + } + + //Draw the appropriate airframe + drawAirframe(frameType); +} + +void ConfigMultiRotorWidget::drawAirframe(QString frameType){ + + invertMotors = m_aircraft->MultirotorRevMixercheckBox->isChecked() ? -1:1; + + if (frameType == "Tri" || frameType == "Tricopter Y") { + if(invertMotors > 0) + quad->setElementId("tri"); + else + quad->setElementId("tri_reverse"); + } + else if (frameType == "QuadX" || frameType == "Quad X") { + if(invertMotors > 0) + quad->setElementId("quad-x"); + else + quad->setElementId("quad-x_reverse"); + } + else if (frameType == "QuadP" || frameType == "Quad +") { + if(invertMotors > 0) + quad->setElementId("quad-plus"); + else + quad->setElementId("quad-plus_reverse"); + } + else if (frameType == "Hexa" || frameType == "Hexacopter") + { + if(invertMotors > 0) + quad->setElementId("quad-hexa"); + else + quad->setElementId("quad-hexa_reverse"); + } + else if (frameType == "HexaX" || frameType == "Hexacopter X" ) { + if(invertMotors > 0) + quad->setElementId("quad-hexa-H"); + else + quad->setElementId("quad-hexa-H_reverse"); + } + else if (frameType == "HexaCoax" || frameType == "Hexacopter Y6") + { + if(invertMotors > 0) + quad->setElementId("hexa-coax"); + else + quad->setElementId("hexa-coax_reverse"); + } + else if (frameType == "Octo" || frameType == "Octocopter") + { + if(invertMotors > 0) + quad->setElementId("quad-octo"); + else + quad->setElementId("quad-octo_reverse"); + } + else if (frameType == "OctoV" || frameType == "Octocopter V") + { + if(invertMotors > 0) + quad->setElementId("quad-octo-v"); + else + quad->setElementId("quad-octo-v_reverse"); + } + else if (frameType == "OctoCoaxP" || frameType == "Octo Coax +") + { + if(invertMotors > 0) + quad->setElementId("octo-coax-P"); + else + quad->setElementId("octo-coax-P_reverse"); + + } + else if (frameType == "OctoCoaxX" || frameType == "Octo Coax X") + { + if(invertMotors > 0) + quad->setElementId("octo-coax-X"); + else + quad->setElementId("octo-coax-X_reverse"); } } @@ -259,6 +326,21 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() return channelDesc; } +void ConfigMultiRotorWidget::setYawMixLevel(int value) +{ + if(value<0) + { + m_aircraft->mrYawMixLevel->setValue((-1)*value); + m_aircraft->MultirotorRevMixercheckBox->setChecked(true); + } + else + { + m_aircraft->mrYawMixLevel->setValue(value); + m_aircraft->MultirotorRevMixercheckBox->setChecked(false); + } + +} + @@ -500,7 +582,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( value/1.27 ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( 1-value/1.27 ); + setYawMixLevel( 1-value/1.27 ); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); @@ -526,7 +608,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( value/1.27 ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( 1-value/1.27 ); + setYawMixLevel( 1-value/1.27 ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue( value/1.27); @@ -556,7 +638,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); //change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; @@ -589,7 +671,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); @@ -617,7 +699,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( value/1.27 ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( value/1.27 ); + setYawMixLevel( value/1.27 ); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); @@ -648,7 +730,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); //change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; @@ -660,7 +742,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); //change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; @@ -672,7 +754,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); //change channels channel = m_aircraft->multiMotorChannelBox3->currentIndex() - 1; @@ -705,7 +787,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue( floor(value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - m_aircraft->mrYawMixLevel->setValue( floor(-value/1.27) ); + setYawMixLevel( floor(-value/1.27) ); value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue( floor(value/1.27) ); @@ -732,6 +814,8 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) } } + + drawAirframe(frameType); } @@ -959,7 +1043,8 @@ bool ConfigMultiRotorWidget::setupMultiRotorMixer(double mixerFactors[8][3]) // and enable only the relevant channels: double pFactor = (double)m_aircraft->mrPitchMixLevel->value()/100; double rFactor = (double)m_aircraft->mrRollMixLevel->value()/100; - double yFactor = (double)m_aircraft->mrYawMixLevel->value()/100; + invertMotors = m_aircraft->MultirotorRevMixercheckBox->isChecked() ? -1:1; + double yFactor =invertMotors * (double)m_aircraft->mrYawMixLevel->value()/100; for (int i=0 ; i<8; i++) { if(mmList.at(i)->isEnabled()) { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h index 085aab106..4ce10aae3 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h @@ -64,9 +64,14 @@ private: void setupMotors(QList motorList); void setupQuadMotor(int channel, double roll, double pitch, double yaw); + float invertMotors; + virtual void ResetActuators(GUIConfigDataUnion* configData); static QStringList getChannelDescriptions(); static const QString CHANNELBOXNAME; + void setYawMixLevel(int); + + void drawAirframe(QString multiRotorType); private slots: virtual void setupUI(QString airframeType); diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp index 33106be4c..63de056ad 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.cpp @@ -13,6 +13,7 @@ #include "relaytuningsettings.h" #include "relaytuning.h" #include "stabilizationsettings.h" +#include "hwsettings.h" ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : ConfigTaskWidget(parent) @@ -28,6 +29,9 @@ ConfigAutotuneWidget::ConfigAutotuneWidget(QWidget *parent) : connect(m_autotune->rateTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); connect(m_autotune->attitudeTuning, SIGNAL(valueChanged(int)), this, SLOT(recomputeStabilization())); + addUAVObject("HwSettings"); + addWidget(m_autotune->enableAutoTune); + RelayTuning *relayTuning = RelayTuning::GetInstance(getObjectManager()); Q_ASSERT(relayTuning); if(relayTuning) @@ -134,3 +138,25 @@ void ConfigAutotuneWidget::recomputeStabilization() m_autotune->pitchAttitudeKp->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KP])); m_autotune->pitchAttitudeKi->setText(QString().number(stabSettings.PitchPI[StabilizationSettings::PITCHPI_KI])); } +void ConfigAutotuneWidget::refreshWidgetsValues(UAVObject *obj) +{ + HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + if(obj==hwSettings) + { + bool dirtyBack=isDirty(); + HwSettings::DataFields hwSettingsData = hwSettings->getData(); + m_autotune->enableAutoTune->setChecked( + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] == HwSettings::OPTIONALMODULES_ENABLED); + setDirty(dirtyBack); + } + ConfigTaskWidget::refreshWidgetsValues(obj); +} +void ConfigAutotuneWidget::updateObjectsFromWidgets() +{ + HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); + HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_AUTOTUNE] = + m_autotune->enableAutoTune->isChecked() ? HwSettings::OPTIONALMODULES_ENABLED : HwSettings::OPTIONALMODULES_DISABLED; + hwSettings->setData(hwSettingsData); + ConfigTaskWidget::updateObjectsFromWidgets(); +} diff --git a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h index 7a89ed373..466335d04 100644 --- a/ground/openpilotgcs/src/plugins/config/configautotunewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configautotunewidget.h @@ -51,7 +51,8 @@ private: signals: public slots: - + void refreshWidgetsValues(UAVObject *obj); + void updateObjectsFromWidgets(); private slots: void recomputeStabilization(); void saveStabilization(); diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index ffdb93e66..532ed8b95 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -1,23 +1,32 @@ 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/multirotor-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 - images/TX.svg images/TX2.svg - images/camera.png - images/txpid.png + images/output_selected.png + images/output_normal.png + images/input_selected.png + images/input_normal.png + images/hardware_normal.png + images/hardware_selected.png + images/vehicle_normal.png + images/vehicle_selected.png + images/ins_selected.png + images/ins_normal.png + images/stabilization_selected.png + images/stabilization_normal.png + images/autotune_selected.png + images/autotune_normal.png + images/txpid_selected.png + images/txpid_normal.png + images/camstab_selected.png + images/camstab_normal.png diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp index 37447b01a..a927f2179 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.cpp @@ -25,14 +25,18 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "configgadgetfactory.h" -#include "configgadgetwidget.h" #include "configgadget.h" #include "configgadgetconfiguration.h" #include "configgadgetoptionspage.h" #include +#include +#include +#include +#include ConfigGadgetFactory::ConfigGadgetFactory(QObject *parent) : - IUAVGadgetFactory(QString("ConfigGadget"), tr("Config Gadget"), parent) + IUAVGadgetFactory(QString("ConfigGadget"), tr("Config Gadget"), parent), + gadgetWidget(0) { } @@ -42,7 +46,26 @@ ConfigGadgetFactory::~ConfigGadgetFactory() Core::IUAVGadget* ConfigGadgetFactory::createGadget(QWidget *parent) { - ConfigGadgetWidget* gadgetWidget = new ConfigGadgetWidget(parent); + gadgetWidget = new ConfigGadgetWidget(parent); + + // Add Menu entry + Core::ActionManager* am = Core::ICore::instance()->actionManager(); + Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + + Core::Command* cmd = am->registerAction(new QAction(this), + "ConfigPlugin.ShowInputWizard", + QList() << + Core::Constants::C_GLOBAL_ID); + cmd->setDefaultKeySequence(QKeySequence("Ctrl+R")); + cmd->action()->setText(tr("Radio Setup Wizard")); + + Core::ModeManager::instance()->addAction(cmd, 1); + + ac->appendGroup("Wizard"); + ac->addAction(cmd, "Wizard"); + + connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(startInputWizard())); + return new ConfigGadget(QString("ConfigGadget"), gadgetWidget, parent); } @@ -55,3 +78,12 @@ IOptionsPage *ConfigGadgetFactory::createOptionsPage(IUAVGadgetConfiguration *co { return new ConfigGadgetOptionsPage(qobject_cast(config)); } + +void ConfigGadgetFactory::startInputWizard() +{ + if(gadgetWidget) + { + Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); + gadgetWidget->startInputWizard(); + } +} diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h index 8d371e746..f44f2c02a 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetfactory.h @@ -28,6 +28,9 @@ #define CONFIGGADGETFACTORY_H #include +#include "configgadgetwidget.h" +#include "config_global.h" + namespace Core { class IUAVGadget; @@ -36,7 +39,7 @@ class IUAVGadgetFactory; using namespace Core; -class ConfigGadgetFactory: public Core::IUAVGadgetFactory +class CONFIG_EXPORT ConfigGadgetFactory: public Core::IUAVGadgetFactory { Q_OBJECT public: @@ -47,6 +50,12 @@ public: IUAVGadget *createGadget(QWidget *parent); IUAVGadgetConfiguration *createConfiguration(QSettings* qSettings); IOptionsPage *createOptionsPage(IUAVGadgetConfiguration *config); + +public slots: + void startInputWizard(); + +private: + ConfigGadgetWidget* gadgetWidget; }; #endif // CONFIGGADGETFACTORY_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index f6b64e18c..12276bbca 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -67,33 +67,61 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) // ********************* QWidget *qwd; + QIcon *icon = new QIcon(); + icon->addFile(":/configgadget/images/hardware_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/hardware_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new DefaultHwSettingsWidget(this); - ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, *icon, QString("Hardware")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/vehicle_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/vehicle_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigVehicleTypeWidget(this); - ftw->insertTab(ConfigGadgetWidget::aircraft, qwd, QIcon(":/configgadget/images/Airframe.png"), QString("Aircraft")); + ftw->insertTab(ConfigGadgetWidget::aircraft, qwd, *icon, QString("Vehicle")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/input_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/input_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigInputWidget(this); - ftw->insertTab(ConfigGadgetWidget::input, qwd, QIcon(":/configgadget/images/Transmitter.png"), QString("Input")); + ftw->insertTab(ConfigGadgetWidget::input, qwd, *icon, QString("Input")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/output_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/output_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigOutputWidget(this); - ftw->insertTab(ConfigGadgetWidget::output, qwd, QIcon(":/configgadget/images/Servo.png"), QString("Output")); + ftw->insertTab(ConfigGadgetWidget::output, qwd, *icon, QString("Output")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/ins_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/ins_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new DefaultAttitudeWidget(this); - ftw->insertTab(ConfigGadgetWidget::sensors, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->insertTab(ConfigGadgetWidget::sensors, qwd, *icon, QString("INS")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/stabilization_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/stabilization_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigStabilizationWidget(this); - ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization")); + ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, *icon, QString("Stabilization")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/autotune_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/autotune_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigAutotuneWidget(this); - ftw->insertTab(ConfigGadgetWidget::autotune, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Autotune")); + ftw->insertTab(ConfigGadgetWidget::autotune, qwd, *icon, QString("Autotune")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/camstab_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/camstab_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigCameraStabilizationWidget(this); - ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab")); + ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, *icon, QString("Camera Stab")); + icon = new QIcon(); + icon->addFile(":/configgadget/images/txpid_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/txpid_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigTxPIDWidget(this); - ftw->insertTab(ConfigGadgetWidget::txpid, qwd, QIcon(":/configgadget/images/txpid.png"), QString("TxPID")); + ftw->insertTab(ConfigGadgetWidget::txpid, qwd, *icon, QString("TxPID")); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); // ********************* // Listen to autopilot connection events @@ -103,8 +131,9 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); // And check whether by any chance we are not already connected - if (telMngr->isConnected()) + if (telMngr->isConnected()) { onAutopilotConnect(); + } help = 0; connect(ftw,SIGNAL(currentAboutToShow(int,bool*)),this,SLOT(tabAboutToChange(int,bool*)));//,Qt::BlockingQueuedConnection); @@ -115,7 +144,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) if (pipxStatusObj != NULL ) { connect(pipxStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updatePipXStatus(UAVObject*))); } else { - qDebug() << "Error: Object is unknown (PipXStatus)."; + qDebug() << "Error: Object is unknown (PipXStatus)."; } // Create the timer that is used to timeout the connection to the PipX. @@ -126,25 +155,38 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) ConfigGadgetWidget::~ConfigGadgetWidget() { - // Do nothing - // TODO: properly delete all the tabs in ftw before exiting } +void ConfigGadgetWidget::startInputWizard() +{ + ftw->setCurrentIndex(ConfigGadgetWidget::input); + ConfigInputWidget* inputWidget = dynamic_cast(ftw->getWidget(ConfigGadgetWidget::input)); + Q_ASSERT(inputWidget); + inputWidget->startInputWizard(); +} + void ConfigGadgetWidget::resizeEvent(QResizeEvent *event) { - QWidget::resizeEvent(event); } void ConfigGadgetWidget::onAutopilotDisconnect() { ftw->setCurrentIndex(ConfigGadgetWidget::hardware); ftw->removeTab(ConfigGadgetWidget::sensors); + + QIcon *icon = new QIcon(); + icon->addFile(":/configgadget/images/ins_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/ins_selected.png", QSize(), QIcon::Selected, QIcon::Off); QWidget *qwd = new DefaultAttitudeWidget(this); - ftw->insertTab(ConfigGadgetWidget::sensors, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->insertTab(ConfigGadgetWidget::sensors, qwd, *icon, QString("INS")); ftw->removeTab(ConfigGadgetWidget::hardware); + + icon = new QIcon(); + icon->addFile(":/configgadget/images/hardware_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/hardware_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new DefaultHwSettingsWidget(this); - ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, *icon, QString("Hardware")); ftw->setCurrentIndex(ConfigGadgetWidget::hardware); emit autopilotDisconnected(); @@ -164,56 +206,64 @@ void ConfigGadgetWidget::onAutopilotConnect() { // Delete the INS panel, replace with CC Panel: ftw->setCurrentIndex(ConfigGadgetWidget::hardware); ftw->removeTab(ConfigGadgetWidget::sensors); + + QIcon *icon = new QIcon(); + icon->addFile(":/configgadget/images/ins_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/ins_selected.png", QSize(), QIcon::Selected, QIcon::Off); QWidget *qwd = new ConfigCCAttitudeWidget(this); - ftw->insertTab(ConfigGadgetWidget::sensors, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("Attitude")); + ftw->insertTab(ConfigGadgetWidget::sensors, qwd, *icon, QString("INS")); ftw->removeTab(ConfigGadgetWidget::hardware); + + icon = new QIcon(); + icon->addFile(":/configgadget/images/hardware_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/hardware_selected.png", QSize(), QIcon::Selected, QIcon::Off); qwd = new ConfigCCHWWidget(this); - ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, *icon, QString("Hardware")); ftw->setCurrentIndex(ConfigGadgetWidget::hardware); - } else if ((board & 0xff00) == 256 ) { - // Mainboard family - Q_ASSERT(0); - /* - ftw->setCurrentIndex(ConfigGadgetWidget::hardware); - ftw->removeTab(ConfigGadgetWidget::sensors); - QWidget *qwd = new ConfigAHRSWidget(this); - ftw->insertTab(ConfigGadgetWidget::sensors, 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); - */ } else if ((board & 0xff00) == 0x0900) { // Revolution sensor calibration ftw->setCurrentIndex(ConfigGadgetWidget::hardware); ftw->removeTab(ConfigGadgetWidget::sensors); + + QIcon *icon = new QIcon(); + icon->addFile(":/configgadget/images/ins_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/ins_selected.png", QSize(), QIcon::Selected, QIcon::Off); QWidget *qwd = new ConfigRevoWidget(this); - ftw->insertTab(ConfigGadgetWidget::sensors, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("Revo")); + ftw->insertTab(ConfigGadgetWidget::sensors, qwd, *icon, QString("Revo")); ftw->removeTab(ConfigGadgetWidget::hardware); + + icon = new QIcon(); + icon->addFile(":/configgadget/images/hardware_normal.png", QSize(), QIcon::Normal, QIcon::Off); + icon->addFile(":/configgadget/images/hardware_selected.png", QSize(), QIcon::Normal, QIcon::On); qwd = new ConfigProHWWidget(this); - ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, *icon, QString("Hardware")); ftw->setCurrentIndex(ConfigGadgetWidget::hardware); + } else { + //Unknown board + Q_ASSERT(0); } } emit autopilotConnected(); } -void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed) +void ConfigGadgetWidget::tabAboutToChange(int i, bool * proceed) { Q_UNUSED(i); - *proceed=true; - ConfigTaskWidget * wid=qobject_cast(ftw->currentWidget()); - if(!wid) + *proceed = true; + ConfigTaskWidget * wid = qobject_cast(ftw->currentWidget()); + if(!wid) { return; + } if(wid->isDirty()) { int ans=QMessageBox::warning(this,tr("Unsaved changes"),tr("The tab you are leaving has unsaved changes," "if you proceed they will be lost." "Do you still want to proceed?"),QMessageBox::Yes,QMessageBox::No); - if(ans==QMessageBox::No) + if(ans==QMessageBox::No) { *proceed=false; - else + } else { wid->setDirty(false); + } } } diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 5c33a50e0..4f737ff36 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -49,6 +49,7 @@ public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune}; + void startInputWizard(); public slots: void onAutopilotConnect(); diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp old mode 100755 new mode 100644 index f66878779..e16d735d9 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -1,1349 +1,1358 @@ -/** - ****************************************************************************** - * - * @file configinputwidget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup ConfigPlugin Config Plugin - * @{ - * @brief Servo input/output configuration panel for the config gadget - *****************************************************************************/ -/* - * 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 "configinputwidget.h" - -#include "uavtalk/telemetrymanager.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define ACCESS_MIN_MOVE -3 -#define ACCESS_MAX_MOVE 3 -#define STICK_MIN_MOVE -8 -#define STICK_MAX_MOVE 8 - -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardNone),transmitterType(heli),loop(NULL),skipflag(false) -{ - manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); - manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); - flightStatusObj = FlightStatus::GetInstance(getObjectManager()); - receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); - m_config = new Ui_InputWidget(); - m_config->setupUi(this); - - addApplySaveButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); - - ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); - Core::Internal::GeneralSettings * settings=pm->getObject(); - if(!settings->useExpertMode()) - m_config->saveRCInputToRAM->setVisible(false); - - addApplySaveButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); - - //Generate the rows of buttons in the input channel form GUI - unsigned int index=0; - foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) - { - Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); - inputChannelForm * inpForm=new inputChannelForm(this,index==0); - m_config->channelSettings->layout()->addWidget(inpForm); //Add the row to the UI - inpForm->setName(name); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inpForm->ui->channelGroup,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inpForm->ui->channelNumber,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); - addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,index); - ++index; - } - - addUAVObjectToWidgetRelation("ManualControlSettings", "Deadband", m_config->deadband, 0, 0.01f); - - connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); - connect(m_config->stackedWidget,SIGNAL(currentChanged(int)),this,SLOT(disableWizardButton(int))); - connect(m_config->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); - - connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); - connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); - connect(m_config->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); - - m_config->stackedWidget->setCurrentIndex(0); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos1,0,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos2,1,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos3,2,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos4,3,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos5,4,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos6,5,1,true); - addUAVObjectToWidgetRelation("ManualControlSettings","FlightModeNumber",m_config->fmsPosNum); - - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Roll,"Roll"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Roll,"Roll"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Roll,"Roll"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Pitch,"Pitch"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Pitch,"Pitch"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Pitch,"Pitch"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Yaw,"Yaw"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Yaw,"Yaw"); - addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Yaw,"Yaw"); - - addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); - addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",m_config->armTimeout,0,1000); - connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); - connect( ManualControlSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updatePositionSlider())); - enableControls(false); - - populateWidgets(); - refreshWidgetsValues(); - // Connect the help button - connect(m_config->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - - m_config->graphicsView->setScene(new QGraphicsScene(this)); - m_config->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); - m_renderer = new QSvgRenderer(); - QGraphicsScene *l_scene = m_config->graphicsView->scene(); - m_config->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) - { - l_scene->clear(); // Deletes all items contained in the scene as well. - - m_txBackground = new QGraphicsSvgItem(); - // All other items will be clipped to the shape of the background - m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape| - QGraphicsItem::ItemClipsToShape); - m_txBackground->setSharedRenderer(m_renderer); - m_txBackground->setElementId("background"); - l_scene->addItem(m_txBackground); - - m_txMainBody = new QGraphicsSvgItem(); - m_txMainBody->setParentItem(m_txBackground); - m_txMainBody->setSharedRenderer(m_renderer); - m_txMainBody->setElementId("body"); - l_scene->addItem(m_txMainBody); - - m_txLeftStick = new QGraphicsSvgItem(); - m_txLeftStick->setParentItem(m_txBackground); - m_txLeftStick->setSharedRenderer(m_renderer); - m_txLeftStick->setElementId("ljoy"); - - m_txRightStick = new QGraphicsSvgItem(); - m_txRightStick->setParentItem(m_txBackground); - m_txRightStick->setSharedRenderer(m_renderer); - m_txRightStick->setElementId("rjoy"); - - m_txAccess0 = new QGraphicsSvgItem(); - m_txAccess0->setParentItem(m_txBackground); - m_txAccess0->setSharedRenderer(m_renderer); - m_txAccess0->setElementId("access0"); - - m_txAccess1 = new QGraphicsSvgItem(); - m_txAccess1->setParentItem(m_txBackground); - m_txAccess1->setSharedRenderer(m_renderer); - m_txAccess1->setElementId("access1"); - - m_txAccess2 = new QGraphicsSvgItem(); - m_txAccess2->setParentItem(m_txBackground); - m_txAccess2->setSharedRenderer(m_renderer); - m_txAccess2->setElementId("access2"); - - m_txFlightMode = new QGraphicsSvgItem(); - m_txFlightMode->setParentItem(m_txBackground); - m_txFlightMode->setSharedRenderer(m_renderer); - m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setZValue(-10); - - m_txArrows = new QGraphicsSvgItem(); - m_txArrows->setParentItem(m_txBackground); - m_txArrows->setSharedRenderer(m_renderer); - m_txArrows->setElementId("arrows"); - m_txArrows->setVisible(false); - - QRectF orig=m_renderer->boundsOnElement("ljoy"); - QMatrix Matrix = m_renderer->matrixForElement("ljoy"); - orig=Matrix.mapRect(orig); - m_txLeftStickOrig.translate(orig.x(),orig.y()); - m_txLeftStick->setTransform(m_txLeftStickOrig,false); - - orig=m_renderer->boundsOnElement("arrows"); - Matrix = m_renderer->matrixForElement("arrows"); - orig=Matrix.mapRect(orig); - m_txArrowsOrig.translate(orig.x(),orig.y()); - m_txArrows->setTransform(m_txArrowsOrig,false); - - orig=m_renderer->boundsOnElement("body"); - Matrix = m_renderer->matrixForElement("body"); - orig=Matrix.mapRect(orig); - m_txMainBodyOrig.translate(orig.x(),orig.y()); - m_txMainBody->setTransform(m_txMainBodyOrig,false); - - orig=m_renderer->boundsOnElement("flightModeCenter"); - Matrix = m_renderer->matrixForElement("flightModeCenter"); - orig=Matrix.mapRect(orig); - m_txFlightModeCOrig.translate(orig.x(),orig.y()); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - - orig=m_renderer->boundsOnElement("flightModeLeft"); - Matrix = m_renderer->matrixForElement("flightModeLeft"); - orig=Matrix.mapRect(orig); - m_txFlightModeLOrig.translate(orig.x(),orig.y()); - orig=m_renderer->boundsOnElement("flightModeRight"); - Matrix = m_renderer->matrixForElement("flightModeRight"); - orig=Matrix.mapRect(orig); - m_txFlightModeROrig.translate(orig.x(),orig.y()); - - orig=m_renderer->boundsOnElement("rjoy"); - Matrix = m_renderer->matrixForElement("rjoy"); - orig=Matrix.mapRect(orig); - m_txRightStickOrig.translate(orig.x(),orig.y()); - m_txRightStick->setTransform(m_txRightStickOrig,false); - - orig=m_renderer->boundsOnElement("access0"); - Matrix = m_renderer->matrixForElement("access0"); - orig=Matrix.mapRect(orig); - m_txAccess0Orig.translate(orig.x(),orig.y()); - m_txAccess0->setTransform(m_txAccess0Orig,false); - - orig=m_renderer->boundsOnElement("access1"); - Matrix = m_renderer->matrixForElement("access1"); - orig=Matrix.mapRect(orig); - m_txAccess1Orig.translate(orig.x(),orig.y()); - m_txAccess1->setTransform(m_txAccess1Orig,false); - - orig=m_renderer->boundsOnElement("access2"); - Matrix = m_renderer->matrixForElement("access2"); - orig=Matrix.mapRect(orig); - m_txAccess2Orig.translate(orig.x(),orig.y()); - m_txAccess2->setTransform(m_txAccess2Orig,true); - } - m_config->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); - animate=new QTimer(this); - connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); - - heliChannelOrder << ManualControlSettings::CHANNELGROUPS_COLLECTIVE << - ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; - - acroChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE << - ManualControlSettings::CHANNELGROUPS_ROLL << - ManualControlSettings::CHANNELGROUPS_PITCH << - ManualControlSettings::CHANNELGROUPS_YAW << - ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << - ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << - ManualControlSettings::CHANNELGROUPS_ACCESSORY2; -} -void ConfigInputWidget::resetTxControls() -{ - - m_txLeftStick->setTransform(m_txLeftStickOrig,false); - m_txRightStick->setTransform(m_txRightStickOrig,false); - m_txAccess0->setTransform(m_txAccess0Orig,false); - m_txAccess1->setTransform(m_txAccess1Orig,false); - m_txAccess2->setTransform(m_txAccess2Orig,false); - m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - m_txArrows->setVisible(false); -} - -ConfigInputWidget::~ConfigInputWidget() -{ - -} - -void ConfigInputWidget::resizeEvent(QResizeEvent *event) -{ - QWidget::resizeEvent(event); - m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); -} - -void ConfigInputWidget::openHelp() -{ - - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode) ); -} -void ConfigInputWidget::goToWizard() -{ - QMessageBox msgBox; - msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); - msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually when the wizard is finished.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - wizardSetUpStep(wizardWelcome); - m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); -} - -void ConfigInputWidget::disableWizardButton(int value) -{ - if(value!=0) - m_config->configurationWizard->setVisible(false); - else - m_config->configurationWizard->setVisible(true); -} - -void ConfigInputWidget::wzCancel() -{ - dimOtherControls(false); - manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); - m_config->stackedWidget->setCurrentIndex(0); - - if(wizardStep != wizardNone) - wizardTearDownStep(wizardStep); - wizardStep=wizardNone; - m_config->stackedWidget->setCurrentIndex(0); - - // Load settings back from beginning of wizard - manualSettingsObj->setData(previousManualSettingsData); -} - -void ConfigInputWidget::wzNext() -{ - // In identify sticks mode the next button can indicate - // channel advance - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) - wizardTearDownStep(wizardStep); - - // State transitions for next button - switch(wizardStep) { - case wizardWelcome: - wizardSetUpStep(wizardChooseMode); - break; - case wizardChooseMode: - wizardSetUpStep(wizardChooseType); - break; - case wizardChooseType: - wizardSetUpStep(wizardIdentifySticks); - break; - case wizardIdentifySticks: - nextChannel(); - if(currentChannelNum==-1) { // Gone through all channels - wizardTearDownStep(wizardIdentifySticks); - wizardSetUpStep(wizardIdentifyCenter); - } - break; - case wizardIdentifyCenter: - wizardSetUpStep(wizardIdentifyLimits); - break; - case wizardIdentifyLimits: - wizardSetUpStep(wizardIdentifyInverted); - break; - case wizardIdentifyInverted: - wizardSetUpStep(wizardFinish); - break; - case wizardFinish: - wizardStep=wizardNone; - m_config->stackedWidget->setCurrentIndex(0); - break; - default: - Q_ASSERT(0); - } -} - -void ConfigInputWidget::wzBack() -{ - if(wizardStep != wizardNone && - wizardStep != wizardIdentifySticks) - wizardTearDownStep(wizardStep); - - // State transitions for next button - switch(wizardStep) { - case wizardChooseMode: - wizardSetUpStep(wizardWelcome); - break; - case wizardChooseType: - wizardSetUpStep(wizardChooseMode); - break; - case wizardIdentifySticks: - prevChannel(); - if(currentChannelNum == -1) { - wizardTearDownStep(wizardIdentifySticks); - wizardSetUpStep(wizardChooseType); - } - break; - case wizardIdentifyCenter: - wizardSetUpStep(wizardIdentifySticks); - break; - case wizardIdentifyLimits: - wizardSetUpStep(wizardIdentifyCenter); - break; - case wizardIdentifyInverted: - wizardSetUpStep(wizardIdentifyLimits); - break; - case wizardFinish: - wizardSetUpStep(wizardIdentifyInverted); - break; - default: - Q_ASSERT(0); - } -} - -void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) -{ - switch(step) { - case wizardWelcome: - foreach(QPointer wd,extraWidgets) - { - if(!wd.isNull()) - delete wd; - } - extraWidgets.clear(); - m_config->graphicsView->setVisible(false); - setTxMovement(nothing); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; - previousManualSettingsData = manualSettingsData; - manualSettingsObj->setData(manualSettingsData); - m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" - "Please follow the instructions on the screen and only move your controls when asked to.\n" - "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" - "You can press 'back' at any time to return to the previous screeen or press 'Cancel' to quit the wizard.\n")); - m_config->stackedWidget->setCurrentIndex(1); - m_config->wzBack->setEnabled(false); - break; - case wizardChooseMode: - { - m_config->graphicsView->setVisible(true); - m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); - setTxMovement(nothing); - m_config->wzText->setText(tr("Please choose your transmitter type.\n" - "Mode 1 means your throttle stick is on the right.\n" - "Mode 2 means your throttle stick is on the left.\n")); - m_config->wzBack->setEnabled(true); - QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); - QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); - mode2->setChecked(true); - extraWidgets.clear(); - extraWidgets.append(mode1); - extraWidgets.append(mode2); - m_config->checkBoxesLayout->layout()->addWidget(mode1); - m_config->checkBoxesLayout->layout()->addWidget(mode2); - } - break; - case wizardChooseType: - { - m_config->wzText->setText(tr("Please choose your transmitter mode.\n" - "Acro means normal transmitter.\n" - "Heli means there is a collective pitch and throttle input.\n" - "If you are using a heli transmitter please engage throttle hold now.\n")); - m_config->wzBack->setEnabled(true); - QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); - QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); - typeAcro->setChecked(true); - typeHeli->setChecked(false); - extraWidgets.clear(); - extraWidgets.append(typeAcro); - extraWidgets.append(typeHeli); - m_config->checkBoxesLayout->layout()->addWidget(typeAcro); - m_config->checkBoxesLayout->layout()->addWidget(typeHeli); - wizardStep=wizardChooseType; - } - break; - case wizardIdentifySticks: - usedChannels.clear(); - currentChannelNum=-1; - nextChannel(); - manualSettingsData=manualSettingsObj->getData(); - connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - m_config->wzNext->setEnabled(false); - break; - case wizardIdentifyCenter: - setTxMovement(centerAll); - m_config->wzText->setText(QString(tr("Please center all controls and press next when ready (if your FlightMode switch has only two positions, leave it in either position)."))); - break; - case wizardIdentifyLimits: - { - accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(),0); - accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(),1); - accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(),2); - setTxMovement(nothing); - m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready."))); - fastMdata(); - manualSettingsData=manualSettingsObj->getData(); - for(uint i=0;igetField("ChannelMax")->getElementNames().length(); index++) - { - QString name = manualSettingsObj->getField("ChannelMax")->getElementNames().at(index); - if(!name.contains("Access") && !name.contains("Flight")) - { - QCheckBox * cb=new QCheckBox(name,this); - // Make sure checked status matches current one - cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]); - - extraWidgets.append(cb); - m_config->checkBoxesLayout->layout()->addWidget(cb); - - connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); - } - } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - m_config->wzText->setText(QString(tr("Please check the picture below and correct all the sticks which show an inverted movement, press next when ready."))); - fastMdata(); - break; - case wizardFinish: - dimOtherControls(false); - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture mimics your sticks movement.\n" - "These new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can save the configuration."))); - fastMdata(); - - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); - if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || - (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) - { - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]=manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ - (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; - } - manualSettingsObj->setData(manualSettingsData); - break; - default: - Q_ASSERT(0); - } - wizardStep = step; -} - -void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) -{ - QRadioButton * mode, * type; - Q_ASSERT(step == wizardStep); - switch(step) { - case wizardWelcome: - break; - case wizardChooseMode: - mode=qobject_cast(extraWidgets.at(0)); - if(mode->isChecked()) - transmitterMode=mode1; - else - transmitterMode=mode2; - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); - break; - case wizardChooseType: - type=qobject_cast(extraWidgets.at(0)); - if(type->isChecked()) - transmitterType=acro; - else - transmitterType=heli; - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); - break; - case wizardIdentifySticks: - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - m_config->wzNext->setEnabled(true); - setTxMovement(nothing); - break; - case wizardIdentifyCenter: - manualCommandData=manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - for(unsigned int i=0;isetData(manualSettingsData); - setTxMovement(nothing); - break; - case wizardIdentifyLimits: - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - manualSettingsObj->setData(manualSettingsData); - restoreMdata(); - setTxMovement(nothing); - break; - case wizardIdentifyInverted: - dimOtherControls(false); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); - delete cb; - } - } - extraWidgets.clear(); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - restoreMdata(); - break; - case wizardFinish: - dimOtherControls(false); - setTxMovement(nothing); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - restoreMdata(); - break; - default: - Q_ASSERT(0); - } -} - -/** - * Set manual control command to fast updates - */ -void ConfigInputWidget::fastMdata() -{ - manualControlMdata = manualCommandObj->getMetadata(); - UAVObject::Metadata mdata = manualControlMdata; - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 150; - manualCommandObj->setMetadata(mdata); -} - -/** - * Restore previous update settings for manual control data - */ -void ConfigInputWidget::restoreMdata() -{ - manualCommandObj->setMetadata(manualControlMdata); -} - -/** - * Set the display to indicate which channel the person should move - */ -void ConfigInputWidget::setChannel(int newChan) -{ - if(newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) - m_config->wzText->setText(QString(tr("Please enable the throttle hold mode and move the collective pitch stick."))); - else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) - m_config->wzText->setText(QString(tr("Please toggle the flight mode switch. For switches you may have to repeat this rapidly."))); - else if((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) - m_config->wzText->setText(QString(tr("Please disable throttle hold mode and move the throttle stick."))); - else - m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" - "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); - - if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || - manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { - m_config->wzNext->setEnabled(true); - m_config->wzText->setText(m_config->wzText->text() + tr(" or click next to skip this channel.")); - } else - m_config->wzNext->setEnabled(false); - - setMoveFromCommand(newChan); - - currentChannelNum = newChan; - channelDetected = false; -} - -/** - * Unfortunately order of channel should be different in different conditions. Selects - * next channel based on heli or acro mode - */ -void ConfigInputWidget::nextChannel() -{ - QList order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder; - - if(currentChannelNum == -1) { - setChannel(order[0]); - return; - } - for (int i = 0; i < order.length() - 1; i++) { - if(order[i] == currentChannelNum) { - setChannel(order[i+1]); - return; - } - } - currentChannelNum = -1; // hit end of list -} - -/** - * Unfortunately order of channel should be different in different conditions. Selects - * previous channel based on heli or acro mode - */ -void ConfigInputWidget::prevChannel() -{ - QList order = transmitterType == heli ? heliChannelOrder : acroChannelOrder; - - // No previous from unset channel or next state - if(currentChannelNum == -1) - return; - - for (int i = 1; i < order.length(); i++) { - if(order[i] == currentChannelNum) { - setChannel(order[i-1]); - usedChannels.removeLast(); - return; - } - } - currentChannelNum = -1; // hit end of list -} - -void ConfigInputWidget::identifyControls() -{ - static int debounce=0; - - receiverActivityData=receiverActivityObj->getData(); - if(receiverActivityData.ActiveChannel==255) - return; - if(channelDetected) - return; - else - { - receiverActivityData=receiverActivityObj->getData(); - currentChannel.group=receiverActivityData.ActiveGroup; - currentChannel.number=receiverActivityData.ActiveChannel; - if(currentChannel==lastChannel) - ++debounce; - lastChannel.group= currentChannel.group; - lastChannel.number=currentChannel.number; - if(!usedChannels.contains(lastChannel) && debounce>1) - { - channelDetected = true; - debounce=0; - usedChannels.append(lastChannel); - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.ChannelGroups[currentChannelNum]=currentChannel.group; - manualSettingsData.ChannelNumber[currentChannelNum]=currentChannel.number; - manualSettingsObj->setData(manualSettingsData); - } - else - return; - } - - m_config->wzText->clear(); - setTxMovement(nothing); - - QTimer::singleShot(2500, this, SLOT(wzNext())); -} - -void ConfigInputWidget::identifyLimits() -{ - manualCommandData=manualCommandObj->getData(); - for(uint i=0;imanualCommandData.Channel[i]) - manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMax[i]manualCommandData.Channel[i]) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; - if(manualSettingsData.ChannelMin[i]setData(manualSettingsData); -} -void ConfigInputWidget::setMoveFromCommand(int command) -{ - //CHANNELNUMBER_ROLL=0, CHANNELNUMBER_PITCH=1, CHANNELNUMBER_YAW=2, CHANNELNUMBER_THROTTLE=3, CHANNELNUMBER_FLIGHTMODE=4, CHANNELNUMBER_ACCESSORY0=5, CHANNELNUMBER_ACCESSORY1=6, CHANNELNUMBER_ACCESSORY2=7 } ChannelNumberElem; - if(command==ManualControlSettings::CHANNELNUMBER_ROLL) - { - setTxMovement(moveRightHorizontalStick); - } - else if(command==ManualControlSettings::CHANNELNUMBER_PITCH) - { - if(transmitterMode==mode2) - setTxMovement(moveRightVerticalStick); - else - setTxMovement(moveLeftVerticalStick); - } - else if(command==ManualControlSettings::CHANNELNUMBER_YAW) - { - setTxMovement(moveLeftHorizontalStick); - } - else if(command==ManualControlSettings::CHANNELNUMBER_THROTTLE) - { - if(transmitterMode==mode2) - setTxMovement(moveLeftVerticalStick); - else - setTxMovement(moveRightVerticalStick); - } - else if(command==ManualControlSettings::CHANNELNUMBER_COLLECTIVE) - { - if(transmitterMode==mode2) - setTxMovement(moveLeftVerticalStick); - else - setTxMovement(moveRightVerticalStick); - } - else if(command==ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) - { - setTxMovement(moveFlightMode); - } - else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY0) - { - setTxMovement(moveAccess0); - } - else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY1) - { - setTxMovement(moveAccess1); - } - else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY2) - { - setTxMovement(moveAccess2); - } - -} - -void ConfigInputWidget::setTxMovement(txMovements movement) -{ - resetTxControls(); - switch(movement) - { - case moveLeftVerticalStick: - movePos=0; - growing=true; - currentMovement=moveLeftVerticalStick; - animate->start(100); - break; - case moveRightVerticalStick: - movePos=0; - growing=true; - currentMovement=moveRightVerticalStick; - animate->start(100); - break; - case moveLeftHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveLeftHorizontalStick; - animate->start(100); - break; - case moveRightHorizontalStick: - movePos=0; - growing=true; - currentMovement=moveRightHorizontalStick; - animate->start(100); - break; - case moveAccess0: - movePos=0; - growing=true; - currentMovement=moveAccess0; - animate->start(100); - break; - case moveAccess1: - movePos=0; - growing=true; - currentMovement=moveAccess1; - animate->start(100); - break; - case moveAccess2: - movePos=0; - growing=true; - currentMovement=moveAccess2; - animate->start(100); - break; - case moveFlightMode: - movePos=0; - growing=true; - currentMovement=moveFlightMode; - animate->start(1000); - break; - case centerAll: - movePos=0; - currentMovement=centerAll; - animate->start(1000); - break; - case moveAll: - movePos=0; - growing=true; - currentMovement=moveAll; - animate->start(50); - break; - case nothing: - movePos=0; - animate->stop(); - break; - default: - break; - } -} - -void ConfigInputWidget::moveTxControls() -{ - QTransform trans; - QGraphicsItem * item; - txMovementType move; - int limitMax; - int limitMin; - static bool auxFlag=false; - switch(currentMovement) - { - case moveLeftVerticalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; - break; - case moveRightVerticalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=vertical; - break; - case moveLeftHorizontalStick: - item=m_txLeftStick; - trans=m_txLeftStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; - break; - case moveRightHorizontalStick: - item=m_txRightStick; - trans=m_txRightStickOrig; - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=horizontal; - break; - case moveAccess0: - item=m_txAccess0; - trans=m_txAccess0Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; - break; - case moveAccess1: - item=m_txAccess1; - trans=m_txAccess1Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; - break; - case moveAccess2: - item=m_txAccess2; - trans=m_txAccess2Orig; - limitMax=ACCESS_MAX_MOVE; - limitMin=ACCESS_MIN_MOVE; - move=horizontal; - break; - case moveFlightMode: - item=m_txFlightMode; - move=jump; - break; - case centerAll: - item=m_txArrows; - move=jump; - break; - case moveAll: - limitMax=STICK_MAX_MOVE; - limitMin=STICK_MIN_MOVE; - move=mix; - break; - default: - break; - } - if(move==vertical) - item->setTransform(trans.translate(0,movePos*10),false); - else if(move==horizontal) - item->setTransform(trans.translate(movePos*10,0),false); - else if(move==jump) - { - if(item==m_txArrows) - { - m_txArrows->setVisible(!m_txArrows->isVisible()); - } - else if(item==m_txFlightMode) - { - QGraphicsSvgItem * svg; - svg=(QGraphicsSvgItem *)item; - if (svg) - { - if(svg->elementId()=="flightModeCenter") - { - if(growing) - { - svg->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else - { - svg->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); - } - } - else if(svg->elementId()=="flightModeRight") - { - growing=false; - svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(svg->elementId()=="flightModeLeft") - { - growing=true; - svg->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - } - } - } - else if(move==mix) - { - trans=m_txAccess0Orig; - m_txAccess0->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess1Orig; - m_txAccess1->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - trans=m_txAccess2Orig; - m_txAccess2->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); - - if(auxFlag) - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(0,movePos*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(0,movePos*10),false); - } - else - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(movePos*10,0),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(movePos*10,0),false); - } - - if(movePos==0) - { - m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if(movePos==ACCESS_MAX_MOVE/2) - { - m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - else if(movePos==ACCESS_MIN_MOVE/2) - { - m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); - } - } - if(move==horizontal || move==vertical ||move==mix) - { - if(movePos==0 && growing) - auxFlag=!auxFlag; - if(growing) - ++movePos; - else - --movePos; - if(movePos>limitMax) - { - movePos=movePos-2; - growing=false; - } - if(movePosgetData(); - flightStatusData=flightStatusObj->getData(); - accessoryDesiredData0=accessoryDesiredObj0->getData(); - accessoryDesiredData1=accessoryDesiredObj1->getData(); - accessoryDesiredData2=accessoryDesiredObj2->getData(); - - if(transmitterMode==mode2) - { - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - } - else - { - trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); - trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); - } - if(flightStatusData.FlightMode==manualSettingsData.FlightModePosition[0]) - { - m_txFlightMode->setElementId("flightModeLeft"); - m_txFlightMode->setTransform(m_txFlightModeLOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[1]) - { - m_txFlightMode->setElementId("flightModeCenter"); - m_txFlightMode->setTransform(m_txFlightModeCOrig,false); - } - else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[2]) - { - m_txFlightMode->setElementId("flightModeRight"); - m_txFlightMode->setTransform(m_txFlightModeROrig,false); - } - m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); - m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); -} - -void ConfigInputWidget::dimOtherControls(bool value) -{ - qreal opac; - if(value) - opac=0.1; - else - opac=1; - m_txAccess0->setOpacity(opac); - m_txAccess1->setOpacity(opac); - m_txAccess2->setOpacity(opac); - m_txFlightMode->setOpacity(opac); -} - -void ConfigInputWidget::enableControls(bool enable) -{ - m_config->configurationWizard->setEnabled(enable); - m_config->runCalibration->setEnabled(enable); - - ConfigTaskWidget::enableControls(enable); - -} - -void ConfigInputWidget::invertControls() -{ - manualSettingsData=manualSettingsObj->getData(); - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - int index = manualSettingsObj->getField("ChannelNumber")->getElementNames().indexOf(cb->text()); - if((cb->isChecked() && (manualSettingsData.ChannelMax[index]>manualSettingsData.ChannelMin[index])) || - (!cb->isChecked() && (manualSettingsData.ChannelMax[index]setData(manualSettingsData); -} - -void ConfigInputWidget::moveFMSlider() -{ - ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); - - float valueScaled; - int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; - int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; - - int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) - { - if (chMax != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); - else - valueScaled = 0; - } - else - { - if (chMin != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); - else - valueScaled = 0; - } - - // Bound and scale FlightMode from [-1..+1] to [0..1] range - if (valueScaled < -1.0) - valueScaled = -1.0; - else - if (valueScaled > 1.0) - valueScaled = 1.0; - - // Convert flightMode value into the switch position in the range [0..N-1] - // This uses the same optimized computation as flight code to be consistent - uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9; - if (pos >= manualSettingsDataPriv.FlightModeNumber) - pos = manualSettingsDataPriv.FlightModeNumber - 1; - m_config->fmsSlider->setValue(pos); -} - -void ConfigInputWidget::updatePositionSlider() -{ - ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - - switch(manualSettingsDataPriv.FlightModeNumber) { - default: - case 6: - m_config->fmsModePos6->setEnabled(true); - // pass through - case 5: - m_config->fmsModePos5->setEnabled(true); - // pass through - case 4: - m_config->fmsModePos4->setEnabled(true); - // pass through - case 3: - m_config->fmsModePos3->setEnabled(true); - // pass through - case 2: - m_config->fmsModePos2->setEnabled(true); - // pass through - case 1: - m_config->fmsModePos1->setEnabled(true); - // pass through - case 0: - break; - } - - switch(manualSettingsDataPriv.FlightModeNumber) { - case 0: - m_config->fmsModePos1->setEnabled(false); - // pass through - case 1: - m_config->fmsModePos2->setEnabled(false); - // pass through - case 2: - m_config->fmsModePos3->setEnabled(false); - // pass through - case 3: - m_config->fmsModePos4->setEnabled(false); - // pass through - case 4: - m_config->fmsModePos5->setEnabled(false); - // pass through - case 5: - m_config->fmsModePos6->setEnabled(false); - // pass through - case 6: - default: - break; - } -} - -void ConfigInputWidget::updateCalibration() -{ - manualCommandData=manualCommandObj->getData(); - for(uint i=0;imanualCommandData.Channel[i]) || - (reverse[i] && manualSettingsData.ChannelMin[i]manualCommandData.Channel[i])) - manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; - manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; - } - - manualSettingsObj->setData(manualSettingsData); - manualSettingsObj->updated(); -} - -void ConfigInputWidget::simpleCalibration(bool enable) -{ - if (enable) { - m_config->configurationWizard->setEnabled(false); - - QMessageBox msgBox; - msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); - msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually when the wizard is finished.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - - manualCommandData = manualCommandObj->getData(); - - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; - manualSettingsObj->setData(manualSettingsData); - - for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { - reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; - manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; - manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; - manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; - } - - fastMdata(); - - connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); - } else { - m_config->configurationWizard->setEnabled(true); - - manualCommandData = manualCommandObj->getData(); - manualSettingsData = manualSettingsObj->getData(); - - restoreMdata(); - - for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) - manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; - - // Force flight mode neutral to middle - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] = - (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; - - // Force throttle to be near min - manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ - ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); - - manualSettingsObj->setData(manualSettingsData); - - disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); - } -} +/** + ****************************************************************************** + * + * @file configinputwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Servo input/output configuration panel for the config gadget + *****************************************************************************/ +/* + * 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 "configinputwidget.h" + +#include "uavtalk/telemetrymanager.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define ACCESS_MIN_MOVE -3 +#define ACCESS_MAX_MOVE 3 +#define STICK_MIN_MOVE -8 +#define STICK_MAX_MOVE 8 + +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardNone),transmitterType(heli),loop(NULL),skipflag(false) +{ + manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + flightStatusObj = FlightStatus::GetInstance(getObjectManager()); + receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); + m_config = new Ui_InputWidget(); + m_config->setupUi(this); + + addApplySaveButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); + + ExtensionSystem::PluginManager *pm=ExtensionSystem::PluginManager::instance(); + Core::Internal::GeneralSettings * settings=pm->getObject(); + if(!settings->useExpertMode()) + m_config->saveRCInputToRAM->setVisible(false); + + addApplySaveButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); + + //Generate the rows of buttons in the input channel form GUI + unsigned int index=0; + foreach (QString name, manualSettingsObj->getField("ChannelNumber")->getElementNames()) + { + Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); + inputChannelForm * inpForm=new inputChannelForm(this,index==0); + m_config->channelSettings->layout()->addWidget(inpForm); //Add the row to the UI + inpForm->setName(name); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inpForm->ui->channelGroup,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inpForm->ui->channelNumber,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inpForm->ui->channelMin,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inpForm->ui->channelNeutral,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inpForm->ui->channelMax,index); + ++index; + } + + addUAVObjectToWidgetRelation("ManualControlSettings", "Deadband", m_config->deadband, 0, 0.01f); + + connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); + connect(m_config->stackedWidget,SIGNAL(currentChanged(int)),this,SLOT(disableWizardButton(int))); + connect(m_config->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); + + connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); + connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); + connect(m_config->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); + + m_config->stackedWidget->setCurrentIndex(0); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos1,0,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos2,1,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos3,2,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos4,3,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos5,4,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos6,5,1,true); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModeNumber",m_config->fmsPosNum); + + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Yaw,"Yaw"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Yaw,"Yaw"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Yaw,"Yaw"); + + addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); + addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",m_config->armTimeout,0,1000); + connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); + connect( ManualControlSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updatePositionSlider())); + enableControls(false); + + populateWidgets(); + refreshWidgetsValues(); + // Connect the help button + connect(m_config->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + + m_config->graphicsView->setScene(new QGraphicsScene(this)); + m_config->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + m_renderer = new QSvgRenderer(); + QGraphicsScene *l_scene = m_config->graphicsView->scene(); + m_config->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) + { + l_scene->clear(); // Deletes all items contained in the scene as well. + + m_txBackground = new QGraphicsSvgItem(); + // All other items will be clipped to the shape of the background + m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + QGraphicsItem::ItemClipsToShape); + m_txBackground->setSharedRenderer(m_renderer); + m_txBackground->setElementId("background"); + l_scene->addItem(m_txBackground); + + m_txMainBody = new QGraphicsSvgItem(); + m_txMainBody->setParentItem(m_txBackground); + m_txMainBody->setSharedRenderer(m_renderer); + m_txMainBody->setElementId("body"); + l_scene->addItem(m_txMainBody); + + m_txLeftStick = new QGraphicsSvgItem(); + m_txLeftStick->setParentItem(m_txBackground); + m_txLeftStick->setSharedRenderer(m_renderer); + m_txLeftStick->setElementId("ljoy"); + + m_txRightStick = new QGraphicsSvgItem(); + m_txRightStick->setParentItem(m_txBackground); + m_txRightStick->setSharedRenderer(m_renderer); + m_txRightStick->setElementId("rjoy"); + + m_txAccess0 = new QGraphicsSvgItem(); + m_txAccess0->setParentItem(m_txBackground); + m_txAccess0->setSharedRenderer(m_renderer); + m_txAccess0->setElementId("access0"); + + m_txAccess1 = new QGraphicsSvgItem(); + m_txAccess1->setParentItem(m_txBackground); + m_txAccess1->setSharedRenderer(m_renderer); + m_txAccess1->setElementId("access1"); + + m_txAccess2 = new QGraphicsSvgItem(); + m_txAccess2->setParentItem(m_txBackground); + m_txAccess2->setSharedRenderer(m_renderer); + m_txAccess2->setElementId("access2"); + + m_txFlightMode = new QGraphicsSvgItem(); + m_txFlightMode->setParentItem(m_txBackground); + m_txFlightMode->setSharedRenderer(m_renderer); + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setZValue(-10); + + m_txArrows = new QGraphicsSvgItem(); + m_txArrows->setParentItem(m_txBackground); + m_txArrows->setSharedRenderer(m_renderer); + m_txArrows->setElementId("arrows"); + m_txArrows->setVisible(false); + + QRectF orig=m_renderer->boundsOnElement("ljoy"); + QMatrix Matrix = m_renderer->matrixForElement("ljoy"); + orig=Matrix.mapRect(orig); + m_txLeftStickOrig.translate(orig.x(),orig.y()); + m_txLeftStick->setTransform(m_txLeftStickOrig,false); + + orig=m_renderer->boundsOnElement("arrows"); + Matrix = m_renderer->matrixForElement("arrows"); + orig=Matrix.mapRect(orig); + m_txArrowsOrig.translate(orig.x(),orig.y()); + m_txArrows->setTransform(m_txArrowsOrig,false); + + orig=m_renderer->boundsOnElement("body"); + Matrix = m_renderer->matrixForElement("body"); + orig=Matrix.mapRect(orig); + m_txMainBodyOrig.translate(orig.x(),orig.y()); + m_txMainBody->setTransform(m_txMainBodyOrig,false); + + orig=m_renderer->boundsOnElement("flightModeCenter"); + Matrix = m_renderer->matrixForElement("flightModeCenter"); + orig=Matrix.mapRect(orig); + m_txFlightModeCOrig.translate(orig.x(),orig.y()); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + + orig=m_renderer->boundsOnElement("flightModeLeft"); + Matrix = m_renderer->matrixForElement("flightModeLeft"); + orig=Matrix.mapRect(orig); + m_txFlightModeLOrig.translate(orig.x(),orig.y()); + orig=m_renderer->boundsOnElement("flightModeRight"); + Matrix = m_renderer->matrixForElement("flightModeRight"); + orig=Matrix.mapRect(orig); + m_txFlightModeROrig.translate(orig.x(),orig.y()); + + orig=m_renderer->boundsOnElement("rjoy"); + Matrix = m_renderer->matrixForElement("rjoy"); + orig=Matrix.mapRect(orig); + m_txRightStickOrig.translate(orig.x(),orig.y()); + m_txRightStick->setTransform(m_txRightStickOrig,false); + + orig=m_renderer->boundsOnElement("access0"); + Matrix = m_renderer->matrixForElement("access0"); + orig=Matrix.mapRect(orig); + m_txAccess0Orig.translate(orig.x(),orig.y()); + m_txAccess0->setTransform(m_txAccess0Orig,false); + + orig=m_renderer->boundsOnElement("access1"); + Matrix = m_renderer->matrixForElement("access1"); + orig=Matrix.mapRect(orig); + m_txAccess1Orig.translate(orig.x(),orig.y()); + m_txAccess1->setTransform(m_txAccess1Orig,false); + + orig=m_renderer->boundsOnElement("access2"); + Matrix = m_renderer->matrixForElement("access2"); + orig=Matrix.mapRect(orig); + m_txAccess2Orig.translate(orig.x(),orig.y()); + m_txAccess2->setTransform(m_txAccess2Orig,true); + } + m_config->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); + animate=new QTimer(this); + connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); + + heliChannelOrder << ManualControlSettings::CHANNELGROUPS_COLLECTIVE << + ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + + acroChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; +} +void ConfigInputWidget::resetTxControls() +{ + + m_txLeftStick->setTransform(m_txLeftStickOrig,false); + m_txRightStick->setTransform(m_txRightStickOrig,false); + m_txAccess0->setTransform(m_txAccess0Orig,false); + m_txAccess1->setTransform(m_txAccess1Orig,false); + m_txAccess2->setTransform(m_txAccess2Orig,false); + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txArrows->setVisible(false); +} + +ConfigInputWidget::~ConfigInputWidget() +{ + +} + +void ConfigInputWidget::resizeEvent(QResizeEvent *event) +{ + QWidget::resizeEvent(event); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); +} + +void ConfigInputWidget::openHelp() +{ + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/x/04Cf", QUrl::StrictMode) ); +} + +void ConfigInputWidget::goToWizard() +{ + QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); + msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually " + "when the wizard is finished. After the last step of the " + "wizard you will be taken to the Arming Settings screen.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + + // Set correct tab visible before starting wizard. + if(m_config->tabWidget->currentIndex() != 0) { + m_config->tabWidget->setCurrentIndex(0); + } + wizardSetUpStep(wizardWelcome); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); +} + +void ConfigInputWidget::disableWizardButton(int value) +{ + if(value!=0) + m_config->configurationWizard->setVisible(false); + else + m_config->configurationWizard->setVisible(true); +} + +void ConfigInputWidget::wzCancel() +{ + dimOtherControls(false); + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + m_config->stackedWidget->setCurrentIndex(0); + + if(wizardStep != wizardNone) + wizardTearDownStep(wizardStep); + wizardStep=wizardNone; + m_config->stackedWidget->setCurrentIndex(0); + + // Load settings back from beginning of wizard + manualSettingsObj->setData(previousManualSettingsData); +} + +void ConfigInputWidget::wzNext() +{ + // In identify sticks mode the next button can indicate + // channel advance + if(wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) + wizardTearDownStep(wizardStep); + + // State transitions for next button + switch(wizardStep) { + case wizardWelcome: + wizardSetUpStep(wizardChooseMode); + break; + case wizardChooseMode: + wizardSetUpStep(wizardChooseType); + break; + case wizardChooseType: + wizardSetUpStep(wizardIdentifySticks); + break; + case wizardIdentifySticks: + nextChannel(); + if(currentChannelNum==-1) { // Gone through all channels + wizardTearDownStep(wizardIdentifySticks); + wizardSetUpStep(wizardIdentifyCenter); + } + break; + case wizardIdentifyCenter: + wizardSetUpStep(wizardIdentifyLimits); + break; + case wizardIdentifyLimits: + wizardSetUpStep(wizardIdentifyInverted); + break; + case wizardIdentifyInverted: + wizardSetUpStep(wizardFinish); + break; + case wizardFinish: + wizardStep=wizardNone; + m_config->stackedWidget->setCurrentIndex(0); + m_config->tabWidget->setCurrentIndex(2); + break; + default: + Q_ASSERT(0); + } +} + +void ConfigInputWidget::wzBack() +{ + if(wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) + wizardTearDownStep(wizardStep); + + // State transitions for next button + switch(wizardStep) { + case wizardChooseMode: + wizardSetUpStep(wizardWelcome); + break; + case wizardChooseType: + wizardSetUpStep(wizardChooseMode); + break; + case wizardIdentifySticks: + prevChannel(); + if(currentChannelNum == -1) { + wizardTearDownStep(wizardIdentifySticks); + wizardSetUpStep(wizardChooseType); + } + break; + case wizardIdentifyCenter: + wizardSetUpStep(wizardIdentifySticks); + break; + case wizardIdentifyLimits: + wizardSetUpStep(wizardIdentifyCenter); + break; + case wizardIdentifyInverted: + wizardSetUpStep(wizardIdentifyLimits); + break; + case wizardFinish: + wizardSetUpStep(wizardIdentifyInverted); + break; + default: + Q_ASSERT(0); + } +} + +void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) +{ + switch(step) { + case wizardWelcome: + foreach(QPointer wd,extraWidgets) + { + if(!wd.isNull()) + delete wd; + } + extraWidgets.clear(); + m_config->graphicsView->setVisible(false); + setTxMovement(nothing); + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + previousManualSettingsData = manualSettingsData; + manualSettingsObj->setData(manualSettingsData); + m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" + "Please follow the instructions on the screen and only move your controls when asked to.\n" + "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" + "You can press 'back' at any time to return to the previous screeen or press 'Cancel' to quit the wizard.\n")); + m_config->stackedWidget->setCurrentIndex(1); + m_config->wzBack->setEnabled(false); + break; + case wizardChooseMode: + { + m_config->graphicsView->setVisible(true); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); + setTxMovement(nothing); + m_config->wzText->setText(tr("Please choose your transmitter type.\n" + "Mode 1 means your throttle stick is on the right.\n" + "Mode 2 means your throttle stick is on the left.\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); + QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); + mode2->setChecked(true); + extraWidgets.clear(); + extraWidgets.append(mode1); + extraWidgets.append(mode2); + m_config->checkBoxesLayout->layout()->addWidget(mode1); + m_config->checkBoxesLayout->layout()->addWidget(mode2); + } + break; + case wizardChooseType: + { + m_config->wzText->setText(tr("Please choose your transmitter mode.\n" + "Acro means normal transmitter.\n" + "Heli means there is a collective pitch and throttle input.\n" + "If you are using a heli transmitter please engage throttle hold now.\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); + QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); + typeAcro->setChecked(true); + typeHeli->setChecked(false); + extraWidgets.clear(); + extraWidgets.append(typeAcro); + extraWidgets.append(typeHeli); + m_config->checkBoxesLayout->layout()->addWidget(typeAcro); + m_config->checkBoxesLayout->layout()->addWidget(typeHeli); + wizardStep=wizardChooseType; + } + break; + case wizardIdentifySticks: + usedChannels.clear(); + currentChannelNum=-1; + nextChannel(); + manualSettingsData=manualSettingsObj->getData(); + connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(false); + break; + case wizardIdentifyCenter: + setTxMovement(centerAll); + m_config->wzText->setText(QString(tr("Please center all controls and press next when ready (if your FlightMode switch has only two positions, leave it in either position)."))); + break; + case wizardIdentifyLimits: + { + accessoryDesiredObj0 = AccessoryDesired::GetInstance(getObjectManager(),0); + accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(),1); + accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(),2); + setTxMovement(nothing); + m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready."))); + fastMdata(); + manualSettingsData=manualSettingsObj->getData(); + for(uint i=0;igetField("ChannelMax")->getElementNames().length(); index++) + { + QString name = manualSettingsObj->getField("ChannelMax")->getElementNames().at(index); + if(!name.contains("Access") && !name.contains("Flight")) + { + QCheckBox * cb=new QCheckBox(name,this); + // Make sure checked status matches current one + cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]); + + extraWidgets.append(cb); + m_config->checkBoxesLayout->layout()->addWidget(cb); + + connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + } + } + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + m_config->wzText->setText(QString(tr("Please check the picture below and correct all the sticks which show an inverted movement, press next when ready."))); + fastMdata(); + break; + case wizardFinish: + dimOtherControls(false); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture mimics your sticks movement.\n" + "These new settings aren't saved to the board yet, after pressing next you will go to the Arming Settings " + "screen where you can set your desired arming sequence and save the configuration."))); + fastMdata(); + + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || + (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) + { + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]=manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; + } + manualSettingsObj->setData(manualSettingsData); + break; + default: + Q_ASSERT(0); + } + wizardStep = step; +} + +void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) +{ + QRadioButton * mode, * type; + Q_ASSERT(step == wizardStep); + switch(step) { + case wizardWelcome: + break; + case wizardChooseMode: + mode=qobject_cast(extraWidgets.at(0)); + if(mode->isChecked()) + transmitterMode=mode1; + else + transmitterMode=mode2; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + break; + case wizardChooseType: + type=qobject_cast(extraWidgets.at(0)); + if(type->isChecked()) + transmitterType=acro; + else + transmitterType=heli; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + break; + case wizardIdentifySticks: + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(true); + setTxMovement(nothing); + break; + case wizardIdentifyCenter: + manualCommandData=manualCommandObj->getData(); + manualSettingsData=manualSettingsObj->getData(); + for(unsigned int i=0;isetData(manualSettingsData); + setTxMovement(nothing); + break; + case wizardIdentifyLimits: + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + manualSettingsObj->setData(manualSettingsData); + restoreMdata(); + setTxMovement(nothing); + break; + case wizardIdentifyInverted: + dimOtherControls(false); + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + delete cb; + } + } + extraWidgets.clear(); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + restoreMdata(); + break; + case wizardFinish: + dimOtherControls(false); + setTxMovement(nothing); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + disconnect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + restoreMdata(); + break; + default: + Q_ASSERT(0); + } +} + +/** + * Set manual control command to fast updates + */ +void ConfigInputWidget::fastMdata() +{ + manualControlMdata = manualCommandObj->getMetadata(); + UAVObject::Metadata mdata = manualControlMdata; + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 150; + manualCommandObj->setMetadata(mdata); +} + +/** + * Restore previous update settings for manual control data + */ +void ConfigInputWidget::restoreMdata() +{ + manualCommandObj->setMetadata(manualControlMdata); +} + +/** + * Set the display to indicate which channel the person should move + */ +void ConfigInputWidget::setChannel(int newChan) +{ + if(newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) + m_config->wzText->setText(QString(tr("Please enable the throttle hold mode and move the collective pitch stick."))); + else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) + m_config->wzText->setText(QString(tr("Please toggle the flight mode switch. For switches you may have to repeat this rapidly."))); + else if((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) + m_config->wzText->setText(QString(tr("Please disable throttle hold mode and move the throttle stick."))); + else + m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" + "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); + + if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") || + manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) { + m_config->wzNext->setEnabled(true); + m_config->wzText->setText(m_config->wzText->text() + tr(" or click next to skip this channel.")); + } else + m_config->wzNext->setEnabled(false); + + setMoveFromCommand(newChan); + + currentChannelNum = newChan; + channelDetected = false; +} + +/** + * Unfortunately order of channel should be different in different conditions. Selects + * next channel based on heli or acro mode + */ +void ConfigInputWidget::nextChannel() +{ + QList order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder; + + if(currentChannelNum == -1) { + setChannel(order[0]); + return; + } + for (int i = 0; i < order.length() - 1; i++) { + if(order[i] == currentChannelNum) { + setChannel(order[i+1]); + return; + } + } + currentChannelNum = -1; // hit end of list +} + +/** + * Unfortunately order of channel should be different in different conditions. Selects + * previous channel based on heli or acro mode + */ +void ConfigInputWidget::prevChannel() +{ + QList order = transmitterType == heli ? heliChannelOrder : acroChannelOrder; + + // No previous from unset channel or next state + if(currentChannelNum == -1) + return; + + for (int i = 1; i < order.length(); i++) { + if(order[i] == currentChannelNum) { + setChannel(order[i-1]); + usedChannels.removeLast(); + return; + } + } + currentChannelNum = -1; // hit end of list +} + +void ConfigInputWidget::identifyControls() +{ + static int debounce=0; + + receiverActivityData=receiverActivityObj->getData(); + if(receiverActivityData.ActiveChannel==255) + return; + if(channelDetected) + return; + else + { + receiverActivityData=receiverActivityObj->getData(); + currentChannel.group=receiverActivityData.ActiveGroup; + currentChannel.number=receiverActivityData.ActiveChannel; + if(currentChannel==lastChannel) + ++debounce; + lastChannel.group= currentChannel.group; + lastChannel.number=currentChannel.number; + if(!usedChannels.contains(lastChannel) && debounce>1) + { + channelDetected = true; + debounce=0; + usedChannels.append(lastChannel); + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.ChannelGroups[currentChannelNum]=currentChannel.group; + manualSettingsData.ChannelNumber[currentChannelNum]=currentChannel.number; + manualSettingsObj->setData(manualSettingsData); + } + else + return; + } + + m_config->wzText->clear(); + setTxMovement(nothing); + + QTimer::singleShot(2500, this, SLOT(wzNext())); +} + +void ConfigInputWidget::identifyLimits() +{ + manualCommandData=manualCommandObj->getData(); + for(uint i=0;imanualCommandData.Channel[i]) + manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; + if(manualSettingsData.ChannelMax[i]manualCommandData.Channel[i]) + manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; + if(manualSettingsData.ChannelMin[i]setData(manualSettingsData); +} +void ConfigInputWidget::setMoveFromCommand(int command) +{ + //CHANNELNUMBER_ROLL=0, CHANNELNUMBER_PITCH=1, CHANNELNUMBER_YAW=2, CHANNELNUMBER_THROTTLE=3, CHANNELNUMBER_FLIGHTMODE=4, CHANNELNUMBER_ACCESSORY0=5, CHANNELNUMBER_ACCESSORY1=6, CHANNELNUMBER_ACCESSORY2=7 } ChannelNumberElem; + if(command==ManualControlSettings::CHANNELNUMBER_ROLL) + { + setTxMovement(moveRightHorizontalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_PITCH) + { + if(transmitterMode==mode2) + setTxMovement(moveRightVerticalStick); + else + setTxMovement(moveLeftVerticalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_YAW) + { + setTxMovement(moveLeftHorizontalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_THROTTLE) + { + if(transmitterMode==mode2) + setTxMovement(moveLeftVerticalStick); + else + setTxMovement(moveRightVerticalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_COLLECTIVE) + { + if(transmitterMode==mode2) + setTxMovement(moveLeftVerticalStick); + else + setTxMovement(moveRightVerticalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) + { + setTxMovement(moveFlightMode); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY0) + { + setTxMovement(moveAccess0); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY1) + { + setTxMovement(moveAccess1); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY2) + { + setTxMovement(moveAccess2); + } + +} + +void ConfigInputWidget::setTxMovement(txMovements movement) +{ + resetTxControls(); + switch(movement) + { + case moveLeftVerticalStick: + movePos=0; + growing=true; + currentMovement=moveLeftVerticalStick; + animate->start(100); + break; + case moveRightVerticalStick: + movePos=0; + growing=true; + currentMovement=moveRightVerticalStick; + animate->start(100); + break; + case moveLeftHorizontalStick: + movePos=0; + growing=true; + currentMovement=moveLeftHorizontalStick; + animate->start(100); + break; + case moveRightHorizontalStick: + movePos=0; + growing=true; + currentMovement=moveRightHorizontalStick; + animate->start(100); + break; + case moveAccess0: + movePos=0; + growing=true; + currentMovement=moveAccess0; + animate->start(100); + break; + case moveAccess1: + movePos=0; + growing=true; + currentMovement=moveAccess1; + animate->start(100); + break; + case moveAccess2: + movePos=0; + growing=true; + currentMovement=moveAccess2; + animate->start(100); + break; + case moveFlightMode: + movePos=0; + growing=true; + currentMovement=moveFlightMode; + animate->start(1000); + break; + case centerAll: + movePos=0; + currentMovement=centerAll; + animate->start(1000); + break; + case moveAll: + movePos=0; + growing=true; + currentMovement=moveAll; + animate->start(50); + break; + case nothing: + movePos=0; + animate->stop(); + break; + default: + break; + } +} + +void ConfigInputWidget::moveTxControls() +{ + QTransform trans; + QGraphicsItem * item; + txMovementType move; + int limitMax; + int limitMin; + static bool auxFlag=false; + switch(currentMovement) + { + case moveLeftVerticalStick: + item=m_txLeftStick; + trans=m_txLeftStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=vertical; + break; + case moveRightVerticalStick: + item=m_txRightStick; + trans=m_txRightStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=vertical; + break; + case moveLeftHorizontalStick: + item=m_txLeftStick; + trans=m_txLeftStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=horizontal; + break; + case moveRightHorizontalStick: + item=m_txRightStick; + trans=m_txRightStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=horizontal; + break; + case moveAccess0: + item=m_txAccess0; + trans=m_txAccess0Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveAccess1: + item=m_txAccess1; + trans=m_txAccess1Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveAccess2: + item=m_txAccess2; + trans=m_txAccess2Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveFlightMode: + item=m_txFlightMode; + move=jump; + break; + case centerAll: + item=m_txArrows; + move=jump; + break; + case moveAll: + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=mix; + break; + default: + break; + } + if(move==vertical) + item->setTransform(trans.translate(0,movePos*10),false); + else if(move==horizontal) + item->setTransform(trans.translate(movePos*10,0),false); + else if(move==jump) + { + if(item==m_txArrows) + { + m_txArrows->setVisible(!m_txArrows->isVisible()); + } + else if(item==m_txFlightMode) + { + QGraphicsSvgItem * svg; + svg=(QGraphicsSvgItem *)item; + if (svg) + { + if(svg->elementId()=="flightModeCenter") + { + if(growing) + { + svg->setElementId("flightModeRight"); + m_txFlightMode->setTransform(m_txFlightModeROrig,false); + } + else + { + svg->setElementId("flightModeLeft"); + m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + } + } + else if(svg->elementId()=="flightModeRight") + { + growing=false; + svg->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + else if(svg->elementId()=="flightModeLeft") + { + growing=true; + svg->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + } + } + } + else if(move==mix) + { + trans=m_txAccess0Orig; + m_txAccess0->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + trans=m_txAccess1Orig; + m_txAccess1->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + trans=m_txAccess2Orig; + m_txAccess2->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + + if(auxFlag) + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(0,movePos*10),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(0,movePos*10),false); + } + else + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(movePos*10,0),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(movePos*10,0),false); + } + + if(movePos==0) + { + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + else if(movePos==ACCESS_MAX_MOVE/2) + { + m_txFlightMode->setElementId("flightModeRight"); + m_txFlightMode->setTransform(m_txFlightModeROrig,false); + } + else if(movePos==ACCESS_MIN_MOVE/2) + { + m_txFlightMode->setElementId("flightModeLeft"); + m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + } + } + if(move==horizontal || move==vertical ||move==mix) + { + if(movePos==0 && growing) + auxFlag=!auxFlag; + if(growing) + ++movePos; + else + --movePos; + if(movePos>limitMax) + { + movePos=movePos-2; + growing=false; + } + if(movePosgetData(); + flightStatusData=flightStatusObj->getData(); + accessoryDesiredData0=accessoryDesiredObj0->getData(); + accessoryDesiredData1=accessoryDesiredObj1->getData(); + accessoryDesiredData2=accessoryDesiredObj2->getData(); + + if(transmitterMode==mode2) + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + } + else + { + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + } + if(flightStatusData.FlightMode==manualSettingsData.FlightModePosition[0]) + { + m_txFlightMode->setElementId("flightModeLeft"); + m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + } + else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[1]) + { + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + else if (flightStatusData.FlightMode==manualSettingsData.FlightModePosition[2]) + { + m_txFlightMode->setElementId("flightModeRight"); + m_txFlightMode->setTransform(m_txFlightModeROrig,false); + } + m_txAccess0->setTransform(QTransform(m_txAccess0Orig).translate(accessoryDesiredData0.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); + m_txAccess1->setTransform(QTransform(m_txAccess1Orig).translate(accessoryDesiredData1.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); + m_txAccess2->setTransform(QTransform(m_txAccess2Orig).translate(accessoryDesiredData2.AccessoryVal*ACCESS_MAX_MOVE*10,0),false); +} + +void ConfigInputWidget::dimOtherControls(bool value) +{ + qreal opac; + if(value) + opac=0.1; + else + opac=1; + m_txAccess0->setOpacity(opac); + m_txAccess1->setOpacity(opac); + m_txAccess2->setOpacity(opac); + m_txFlightMode->setOpacity(opac); +} + +void ConfigInputWidget::enableControls(bool enable) +{ + m_config->configurationWizard->setEnabled(enable); + m_config->runCalibration->setEnabled(enable); + + ConfigTaskWidget::enableControls(enable); + +} + +void ConfigInputWidget::invertControls() +{ + manualSettingsData=manualSettingsObj->getData(); + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + int index = manualSettingsObj->getField("ChannelNumber")->getElementNames().indexOf(cb->text()); + if((cb->isChecked() && (manualSettingsData.ChannelMax[index]>manualSettingsData.ChannelMin[index])) || + (!cb->isChecked() && (manualSettingsData.ChannelMax[index]setData(manualSettingsData); +} + +void ConfigInputWidget::moveFMSlider() +{ + ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); + ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData(); + + float valueScaled; + int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; + + int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) + { + if (chMax != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); + else + valueScaled = 0; + } + else + { + if (chMin != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); + else + valueScaled = 0; + } + + // Bound and scale FlightMode from [-1..+1] to [0..1] range + if (valueScaled < -1.0) + valueScaled = -1.0; + else + if (valueScaled > 1.0) + valueScaled = 1.0; + + // Convert flightMode value into the switch position in the range [0..N-1] + // This uses the same optimized computation as flight code to be consistent + uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9; + if (pos >= manualSettingsDataPriv.FlightModeNumber) + pos = manualSettingsDataPriv.FlightModeNumber - 1; + m_config->fmsSlider->setValue(pos); +} + +void ConfigInputWidget::updatePositionSlider() +{ + ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); + + switch(manualSettingsDataPriv.FlightModeNumber) { + default: + case 6: + m_config->fmsModePos6->setEnabled(true); + // pass through + case 5: + m_config->fmsModePos5->setEnabled(true); + // pass through + case 4: + m_config->fmsModePos4->setEnabled(true); + // pass through + case 3: + m_config->fmsModePos3->setEnabled(true); + // pass through + case 2: + m_config->fmsModePos2->setEnabled(true); + // pass through + case 1: + m_config->fmsModePos1->setEnabled(true); + // pass through + case 0: + break; + } + + switch(manualSettingsDataPriv.FlightModeNumber) { + case 0: + m_config->fmsModePos1->setEnabled(false); + // pass through + case 1: + m_config->fmsModePos2->setEnabled(false); + // pass through + case 2: + m_config->fmsModePos3->setEnabled(false); + // pass through + case 3: + m_config->fmsModePos4->setEnabled(false); + // pass through + case 4: + m_config->fmsModePos5->setEnabled(false); + // pass through + case 5: + m_config->fmsModePos6->setEnabled(false); + // pass through + case 6: + default: + break; + } +} + +void ConfigInputWidget::updateCalibration() +{ + manualCommandData=manualCommandObj->getData(); + for(uint i=0;imanualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMin[i]manualCommandData.Channel[i])) + manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + } + + manualSettingsObj->setData(manualSettingsData); + manualSettingsObj->updated(); +} + +void ConfigInputWidget::simpleCalibration(bool enable) +{ + if (enable) { + m_config->configurationWizard->setEnabled(false); + + QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); + msgBox.setDetailedText(tr("You will have to reconfigure the arming settings manually when the wizard is finished.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + + manualCommandData = manualCommandObj->getData(); + + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsObj->setData(manualSettingsData); + + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { + reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } + + fastMdata(); + + connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + } else { + m_config->configurationWizard->setEnabled(true); + + manualCommandData = manualCommandObj->getData(); + manualSettingsData = manualSettingsObj->getData(); + + restoreMdata(); + + for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + + // Force flight mode neutral to middle + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] = + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; + + // Force throttle to be near min + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + + manualSettingsObj->setData(manualSettingsData); + + disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + } +} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 4b28ca491..ab6250e88 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -60,7 +60,7 @@ public: enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; enum txMovementType{vertical,horizontal,jump,mix}; enum txType {acro, heli}; -public slots: + void startInputWizard() { goToWizard(); } private: bool growing; @@ -144,6 +144,7 @@ private: void wizardSetUpStep(enum wizardSteps); void wizardTearDownStep(enum wizardSteps); + private slots: void wzNext(); void wzBack(); @@ -161,11 +162,10 @@ private slots: void invertControls(); void simpleCalibration(bool state); void updateCalibration(); + protected: void resizeEvent(QResizeEvent *event); virtual void enableControls(bool enable); - - }; #endif diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.cpp b/ground/openpilotgcs/src/plugins/config/configplugin.cpp index 30418094c..fd3a30544 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.cpp +++ b/ground/openpilotgcs/src/plugins/config/configplugin.cpp @@ -99,7 +99,7 @@ void ConfigPlugin::extensionsInitialized() void ConfigPlugin::shutdown() { - // Do nothing + // Do nothing } /** diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index 7055851ef..05d17271f 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -215,6 +215,9 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi connect(m_aircraft->ffTestBox2, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); connect(m_aircraft->ffTestBox3, SIGNAL(clicked(bool)), this, SLOT(enableFFTest())); + //Connect the multirotor motor reverse checkbox + connect(m_aircraft->MultirotorRevMixercheckBox, SIGNAL(clicked(bool)), this, SLOT(reverseMultirotorMotor())); + // Connect the help pushbutton connect(m_aircraft->airframeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); enableControls(false); @@ -483,7 +486,7 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject * o) UAVObjectField *field = system->getField(QString("AirframeType")); Q_ASSERT(field); // At this stage, we will need to have some hardcoded settings in this code, this - // is not ideal, but here you go. + // is not ideal, but there you go. QString frameType = field->getValue().toString(); setupAirframeUI(frameType); @@ -766,6 +769,12 @@ void ConfigVehicleTypeWidget::setComboCurrentIndex(QComboBox* box, int index) box->setCurrentIndex(index); } +void ConfigVehicleTypeWidget::reverseMultirotorMotor(){ + QString frameType = m_aircraft->multirotorFrameType->currentText(); + m_multirotor->drawAirframe(frameType); +} + + /** WHAT DOES THIS DO??? */ diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h index 60ef74fbe..e4cea4cc2 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.h @@ -95,6 +95,7 @@ private slots: void enableFFTest(); void openHelp(); + void reverseMultirotorMotor(); protected: void showEvent(QShowEvent *event); diff --git a/ground/openpilotgcs/src/plugins/config/images/autotune_normal.png b/ground/openpilotgcs/src/plugins/config/images/autotune_normal.png new file mode 100644 index 000000000..5c7ca95f1 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/autotune_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/autotune_selected.png b/ground/openpilotgcs/src/plugins/config/images/autotune_selected.png new file mode 100644 index 000000000..53fe7cf0c Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/autotune_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/camstab_normal.png b/ground/openpilotgcs/src/plugins/config/images/camstab_normal.png new file mode 100644 index 000000000..07cf6c6c4 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/camstab_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/camstab_selected.png b/ground/openpilotgcs/src/plugins/config/images/camstab_selected.png new file mode 100644 index 000000000..30b82f0fe Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/camstab_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/hardware_normal.png b/ground/openpilotgcs/src/plugins/config/images/hardware_normal.png new file mode 100644 index 000000000..077d9712e Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/hardware_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/hardware_selected.png b/ground/openpilotgcs/src/plugins/config/images/hardware_selected.png new file mode 100644 index 000000000..83611be21 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/hardware_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/input_normal.png b/ground/openpilotgcs/src/plugins/config/images/input_normal.png new file mode 100644 index 000000000..e5315ced7 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/input_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/input_selected.png b/ground/openpilotgcs/src/plugins/config/images/input_selected.png new file mode 100644 index 000000000..b4c2340ee Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/input_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/ins_normal.png b/ground/openpilotgcs/src/plugins/config/images/ins_normal.png new file mode 100644 index 000000000..335f0ff34 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/ins_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/ins_selected.png b/ground/openpilotgcs/src/plugins/config/images/ins_selected.png new file mode 100644 index 000000000..7e04f144f Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/ins_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg index de5f2bd64..92ec0177e 100644 --- a/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg @@ -14,7 +14,7 @@ version="1.1" inkscape:version="0.48.2 r9819" width="4065.2493" - height="1760.019" + height="3560.019" xml:space="preserve" sodipodi:docname="multirotor-shapes.svg"> \ No newline at end of file + inkscape:connector-curvature="0" /> \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/config/images/output_normal.png b/ground/openpilotgcs/src/plugins/config/images/output_normal.png new file mode 100644 index 000000000..f5754786c Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/output_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/output_selected.png b/ground/openpilotgcs/src/plugins/config/images/output_selected.png new file mode 100644 index 000000000..016272b5e Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/output_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/stabilization_normal.png b/ground/openpilotgcs/src/plugins/config/images/stabilization_normal.png new file mode 100644 index 000000000..7d6c750cd Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/stabilization_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/stabilization_selected.png b/ground/openpilotgcs/src/plugins/config/images/stabilization_selected.png new file mode 100644 index 000000000..7394f23ed Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/stabilization_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/txpid_normal.png b/ground/openpilotgcs/src/plugins/config/images/txpid_normal.png new file mode 100644 index 000000000..79d2d1d7f Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/txpid_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/txpid_selected.png b/ground/openpilotgcs/src/plugins/config/images/txpid_selected.png new file mode 100644 index 000000000..0e4fe57d1 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/txpid_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/vehicle_normal.png b/ground/openpilotgcs/src/plugins/config/images/vehicle_normal.png new file mode 100644 index 000000000..ef96bb78d Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/vehicle_normal.png differ diff --git a/ground/openpilotgcs/src/plugins/config/images/vehicle_selected.png b/ground/openpilotgcs/src/plugins/config/images/vehicle_selected.png new file mode 100644 index 000000000..292c6026e Binary files /dev/null and b/ground/openpilotgcs/src/plugins/config/images/vehicle_selected.png differ diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index f56c8348e..4ff6b33ff 100755 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -7,7 +7,7 @@ 0 0 965 - 713 + 687 @@ -453,6 +453,9 @@ false + + 6 + 12 @@ -597,8 +600,8 @@ 0 0 - 935 - 619 + 937 + 595 @@ -638,12 +641,12 @@ false - - 6 - 12 + + 6 + @@ -4008,12 +4011,12 @@ value as the Kp. Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - 6 - 12 + + 6 + @@ -6934,8 +6937,8 @@ border-radius: 5; 0 0 - 920 - 644 + 540 + 663 @@ -7489,12 +7492,12 @@ border-radius: 5; false - - 6 - 12 + + 6 + @@ -13938,12 +13941,12 @@ border-radius: 5; Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - 6 - 12 + + 6 + @@ -16857,8 +16860,8 @@ border-radius: 5; 0 0 - 935 - 619 + 802 + 607 @@ -19488,12 +19491,12 @@ border-radius: 5; Integral Limits - - 6 - 12 + + 6 + diff --git a/ground/openpilotgcs/src/plugins/config/txpid.ui b/ground/openpilotgcs/src/plugins/config/txpid.ui index 9d4da0865..1c277d0fb 100755 --- a/ground/openpilotgcs/src/plugins/config/txpid.ui +++ b/ground/openpilotgcs/src/plugins/config/txpid.ui @@ -7,7 +7,7 @@ 0 0 789 - 615 + 484 @@ -113,8 +113,8 @@ 0 0 - 759 - 532 + 745 + 469 diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 30061c0c5..8d1e7ff1c 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -101,7 +101,7 @@ void ConnectionManager::init() /** * Method called when the user clicks the "Connect" button */ -bool ConnectionManager::connectDevice() +bool ConnectionManager::connectDevice(DevListItem device) { DevListItem connection_device = findDevice(m_availableDevList->itemData(m_availableDevList->currentIndex(),Qt::ToolTipRole).toString()); if (!connection_device.connection) @@ -126,7 +126,7 @@ bool ConnectionManager::connectDevice() connect(m_connectionDevice.connection, SIGNAL(destroyed(QObject *)), this, SLOT(onConnectionDestroyed(QObject *)), Qt::QueuedConnection); // signal interested plugins that we connected to the device - emit deviceConnected(m_ioDev); + emit deviceConnected(io_dev); m_connectBtn->setText("Disconnect"); m_availableDevList->setEnabled(false); @@ -174,6 +174,7 @@ bool ConnectionManager::disconnectDevice() m_connectionDevice.connection = NULL; m_ioDev = NULL; + emit deviceDisconnected(); m_connectBtn->setText("Connect"); m_availableDevList->setEnabled(true); @@ -230,8 +231,11 @@ void ConnectionManager::onConnectClicked() { // Check if we have a ioDev already created: if (!m_ioDev) - { // connecting - connectDevice(); + { + // connecting to currently selected device + DevListItem device = findDevice(m_availableDevList->itemData(m_availableDevList->currentIndex(), Qt::ToolTipRole).toString()); + if (device.connection) + connectDevice(device); } else { // disconnecting @@ -427,6 +431,9 @@ void ConnectionManager::devChanged(IConnection *connection) updateConnectionDropdown(); + qDebug() << "# devices " << m_devList.count(); + emit availableDevicesChanged(m_devList); + //disable connection button if the liNameif (m_availableDevList->count() > 0) if (m_availableDevList->count() > 0) @@ -445,11 +452,12 @@ void ConnectionManager::updateConnectionDropdown() if(!m_ioDev && d.getConName().startsWith("USB")) { if(m_mainWindow->generalSettings()->autoConnect() || m_mainWindow->generalSettings()->autoSelect()) - m_availableDevList->setCurrentIndex(m_availableDevList->count()-1); + m_availableDevList->setCurrentIndex(m_availableDevList->count() - 1); + if(m_mainWindow->generalSettings()->autoConnect() && polling) { qDebug() << "Automatically opening device"; - connectDevice(); + connectDevice(d); qDebug()<<"ConnectionManager::devChanged autoconnected USB device"; } } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 7e27b3351..2f131d4a3 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -92,7 +92,14 @@ public: void init(); QIODevice* getCurrentConnection() { return m_ioDev; } - DevListItem getCurrentDevice() { return m_connectionDevice;} + DevListItem getCurrentDevice() { return m_connectionDevice; } + DevListItem findDevice(const QString &devName); + + QLinkedList getAvailableDevices() { return m_devList; } + + bool isConnected() { return m_ioDev != 0; } + + bool connectDevice(DevListItem device); bool disconnectDevice(); void suspendPolling(); void resumePolling(); @@ -101,11 +108,12 @@ protected: void updateConnectionList(IConnection *connection); void registerDevice(IConnection *conn, IConnection::device device); void updateConnectionDropdown(); - DevListItem findDevice(const QString &devName); signals: - void deviceConnected(QIODevice *dev); + void deviceConnected(QIODevice *device); void deviceAboutToDisconnect(); + void deviceDisconnected(); + void availableDevicesChanged(const QLinkedList devices); public slots: void telemetryConnected(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp index 663f5e545..39a656de2 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp @@ -39,7 +39,6 @@ #include #include "ui_generalsettings.h" -#include using namespace Utils; using namespace Core::Internal; @@ -113,14 +112,8 @@ void GeneralSettings::fillLanguageBox() const QWidget *GeneralSettings::createPage(QWidget *parent) { m_page = new Ui::GeneralSettings(); - globalSettingsWidget *w = new globalSettingsWidget(parent); - connect(w,SIGNAL(showHidden()),this,SLOT(showHidden())); + QWidget *w = new QWidget(parent); m_page->setupUi(w); - m_page->labelUDP->setVisible(false); - m_page->cbUseUDPMirror->setVisible(false); - m_page->labelExpert->setVisible(false); - m_page->cbExpertMode->setVisible(false); - fillLanguageBox(); connect(m_page->checkAutoConnect,SIGNAL(stateChanged(int)),this,SLOT(slotAutoConnect(int))); m_page->checkBoxSaveOnExit->setChecked(m_saveSettingsOnExit); @@ -263,21 +256,3 @@ void GeneralSettings::slotAutoConnect(int value) else m_page->checkAutoSelect->setEnabled(true); } - -void GeneralSettings::showHidden() -{ - m_page->labelUDP->setVisible(true); - m_page->cbUseUDPMirror->setVisible(true); - m_page->labelExpert->setVisible(true); - m_page->cbExpertMode->setVisible(true); -} - -globalSettingsWidget::globalSettingsWidget(QWidget *parent):QWidget(parent){} - -void globalSettingsWidget::keyPressEvent(QKeyEvent *event) -{ - if(event->key()==Qt::Key_F7) - { - emit showHidden(); - } -} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h index 8b41d9b0f..8bd498fe8 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h @@ -69,7 +69,6 @@ private slots: void resetLanguage(); void showHelpForExternalEditor(); void slotAutoConnect(int); - void showHidden(); private: void fillLanguageBox() const; @@ -86,17 +85,6 @@ private: QList m_codecs; }; -class globalSettingsWidget:public QWidget -{ - Q_OBJECT -public: - globalSettingsWidget(QWidget * parent); -protected: - void keyPressEvent(QKeyEvent *); -signals: - void showHidden(); -}; - } // namespace Internal } // namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/ah.png b/ground/openpilotgcs/src/plugins/coreplugin/images/ah.png index a0be19bed..646f24ad6 100644 Binary files a/ground/openpilotgcs/src/plugins/coreplugin/images/ah.png and b/ground/openpilotgcs/src/plugins/coreplugin/images/ah.png differ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/cog.png b/ground/openpilotgcs/src/plugins/coreplugin/images/cog.png index 67de2c6cc..97b835c58 100644 Binary files a/ground/openpilotgcs/src/plugins/coreplugin/images/cog.png and b/ground/openpilotgcs/src/plugins/coreplugin/images/cog.png differ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/config.png b/ground/openpilotgcs/src/plugins/coreplugin/images/config.png index 5c8213fef..650a00365 100644 Binary files a/ground/openpilotgcs/src/plugins/coreplugin/images/config.png and b/ground/openpilotgcs/src/plugins/coreplugin/images/config.png differ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/joystick.png b/ground/openpilotgcs/src/plugins/coreplugin/images/joystick.png index 62168f56f..960ba9465 100644 Binary files a/ground/openpilotgcs/src/plugins/coreplugin/images/joystick.png and b/ground/openpilotgcs/src/plugins/coreplugin/images/joystick.png differ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/scopes.png b/ground/openpilotgcs/src/plugins/coreplugin/images/scopes.png index 01e933a61..99434575f 100644 Binary files a/ground/openpilotgcs/src/plugins/coreplugin/images/scopes.png and b/ground/openpilotgcs/src/plugins/coreplugin/images/scopes.png differ diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp index aa5356705..85b3c4bde 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp @@ -326,3 +326,13 @@ void ModeManager::setFocusToCurrentMode() widget->setFocus(); } } + +void ModeManager::triggerAction(const QString &actionId) +{ + foreach(Command * command, m_actions.keys()){ + if(command->action()->objectName() == actionId) { + command->action()->trigger(); + break; + } + } +} diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h index ccaf50607..d8fba46bd 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.h @@ -82,6 +82,7 @@ public slots: void activateMode(const QString &id); void activateModeByWorkspaceName(const QString &id); void setFocusToCurrentMode(); + void triggerAction(const QString &actionId); private slots: void objectAdded(QObject *obj); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 485cd7b31..54447b84b 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -293,7 +293,6 @@ double GCSControlGadget::constrain(double value) void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) { -// int state; if ((buttonSettings[number].ActionID>0)&&(buttonSettings[number].FunctionID>0)&&(pressed)) {//this button is configured ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp index cca79a565..87ecef211 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp @@ -301,7 +301,6 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) const int sentence_total = tokenslist.at(1).toInt(); // Number of sentences for full data const int sentence_index = tokenslist.at(2).toInt(); // sentence x of y -// const int sat_count = tokenslist.at(3).toInt(); // Number of satellites in view int sats = (tokenslist.size() - 4) /4; for(int sat = 0; sat < sats; sat++) { diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui index 00a2f60aa..f82b350c3 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui @@ -6,29 +6,14 @@ 0 0 - 403 - 400 + 615 + 577 Form - - 3 - - - 0 - - - 0 - - - 0 - - - 3 - @@ -144,475 +129,569 @@ + + 6 + + + 6 + - - - Attitude data + + + QFrame::NoFrame - - - 3 - - - 3 - - - 3 - - - 3 - - - 0 - - - - - AttitudeRaw (gyro, accels) - - - true - - - true - - - false - - - - 3 - - - 3 - - - 0 - - - 0 - - - - - Refresh rate - - - - - - - ms - - - 10 - - - 100 - - - 20 - - - - - - - - - - AttitudeActual - - - true - - - true - - - false - - - - 3 - - - 3 - - - 0 - - - 0 - - - - - send raw data to board - - - - - - - - 75 - true - - - - - - - use values from simulator - - - true - - - - - - - - - - calculate from AttitudeRaw - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - - - - - - Other data + + QFrame::Plain - - - 3 + + true + + + + + 0 + 0 + 574 + 391 + - - 3 - - - 3 - - - 3 - - - 0 - - - - - HomeLocation - - - true - - - true - - - false - - - - 3 + + + 0 + + + + + Attitude data - - 3 - - - 0 - - - 0 - - - - - Refresh rate - - - - - - - 0 - update once, or every N seconds - - - sec - - - 10 - - - - - - - - - - GPSPosition - - - true - - - true - - - false - - - - 3 - - - 3 - - - 0 - - - 0 - - - - - Refresh rate - - - - - - - ms - - - 100 - - - 2000 - - - 500 - - - - - - - - - - SonarAltitude - - - true - - - true - - - false - - - - 3 - - - 6 - - - 3 - - - 0 - - - 0 - - - - - - - Range detectioon - - - - - - - m - - - 1 - - - 10 - - - 5 - - - - - - - Refresh rate - - - - - - - ms - - - 20 - - - 2000 - - - 10 - - - 50 - - - - - - - - - - - - Map command from simulator - - - true - - - true - - - false - - - - 3 - - - 3 - - - 0 - - - 0 - - - - - - 75 - true - - - - to GCSReciver - - - true - - - - - - - false - - - to ManualCtrll (not implemented) - - - - - - - - - - 6 - - - - - Maximum output rate + + + 12 - - - - - - ms + + 3 - - 10 + + 18 - - 100 + + 3 - - 5 + + 0 - - 15 + + + + + 0 + 0 + + + + AttitudeRaw (gyro, accels) + + + true + + + true + + + false + + + + 6 + + + 12 + + + 0 + + + 12 + + + + + Refresh rate + + + + + + + ms + + + 10 + + + 100 + + + 20 + + + + + + + + + + + 0 + 0 + + + + AttitudeActual + + + true + + + true + + + false + + + + 6 + + + 12 + + + 0 + + + 12 + + + + + send raw data to board + + + + + + + + 75 + true + + + + + + + use values from simulator + + + true + + + + + + + + + + calculate from AttitudeRaw + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 20 + 40 + + + + + + + + + + + Other data + + + + 12 - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - + + 3 + + + 18 + + + 3 + + + 0 + + + + + + 0 + 0 + + + + HomeLocation + + + true + + + true + + + false + + + + 6 + + + 12 + + + 0 + + + 12 + + + + + + 0 + 0 + + + + Refresh rate + + + + + + + + 0 + 0 + + + + 0 - update once, or every N seconds + + + sec + + + 10 + + + + + + + + + + + 0 + 0 + + + + GPSPosition + + + true + + + true + + + false + + + + 6 + + + 12 + + + 0 + + + 12 + + + + + Refresh rate + + + + + + + ms + + + 100 + + + 2000 + + + 500 + + + + + + + + + + + 0 + 0 + + + + SonarAltitude + + + true + + + true + + + false + + + + 3 + + + 6 + + + 3 + + + 0 + + + 0 + + + + + 6 + + + 6 + + + + + Range detection + + + + + + + m + + + 1 + + + 10 + + + 5 + + + + + + + Refresh rate + + + + + + + ms + + + 20 + + + 2000 + + + 10 + + + 50 + + + + + + + + + + + + + 0 + 0 + + + + Map command from simulator + + + true + + + true + + + false + + + + 3 + + + 12 + + + 0 + + + 12 + + + + + + 75 + true + + + + to GCSReciver + + + true + + + + + + + false + + + to ManualCtrll (not implemented) + + + + + + + + + + 6 + + + + + Maximum output rate + + + + + + + ms + + + 10 + + + 100 + + + 5 + + + 15 + + + + + + + + + Qt::Vertical + + + QSizePolicy::Expanding + + + + 20 + 40 + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 19a2a0426..c4e084a89 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -866,8 +866,10 @@ void OPMapGadgetWidget::onTelemetryConnect() setHome(internals::PointLatLng(LLA[0], LLA[1]),LLA[2]); if (m_map) - m_map->SetCurrentPosition(m_home_position.coord); // set the map position - + { + if(m_map->UAV->GetMapFollowType()!=UAVMapFollowType::None) + m_map->SetCurrentPosition(m_home_position.coord); // set the map position + } // *********************** } @@ -2167,6 +2169,7 @@ void OPMapGadgetWidget::on_tbFind_clicked() { pal.setColor( m_widget->leFind->backgroundRole(), Qt::green); m_widget->leFind->setPalette(pal); + m_map->SetZoom(12); } else { @@ -2188,3 +2191,8 @@ void OPMapGadgetWidget::onOverlayOpacityActGroup_triggered(QAction *action) m_map->setOverlayOpacity(action->data().toReal()/100); emit overlayOpacityChanged(action->data().toReal()/100); } + +void OPMapGadgetWidget::on_leFind_returnPressed() +{ + on_tbFind_clicked(); +} diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index c58eade5e..fbac0b958 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -212,6 +212,8 @@ private slots: void on_tbFind_clicked(); void onHomeDoubleClick(HomeItem*); void onOverlayOpacityActGroup_triggered(QAction *action); + void on_leFind_returnPressed(); + private: int m_min_zoom; int m_max_zoom; diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h index 073e1f326..be6625ab0 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadget.h @@ -1,45 +1,45 @@ -/* - * 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 PFDQMLGADGET_H_ -#define PFDQMLQMLGADGET_H_ - -#include -#include "pfdqmlgadgetwidget.h" - -class IUAVGadget; -class QWidget; -class QString; -class PfdQmlGadgetWidget; - -using namespace Core; - -class PfdQmlGadget : public Core::IUAVGadget -{ - Q_OBJECT -public: - PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent = 0); - ~PfdQmlGadget(); - - QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); - -private: - PfdQmlGadgetWidget *m_widget; -}; - - -#endif // PFDQMLQMLGADGET_H_ +/* + * 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 PFDQMLGADGET_H_ +#define PFDQMLQMLGADGET_H_ + +#include +#include "pfdqmlgadgetwidget.h" + +class IUAVGadget; +class QWidget; +class QString; +class PfdQmlGadgetWidget; + +using namespace Core; + +class PfdQmlGadget : public Core::IUAVGadget +{ + Q_OBJECT +public: + PfdQmlGadget(QString classId, PfdQmlGadgetWidget *widget, QWidget *parent = 0); + ~PfdQmlGadget(); + + QWidget *widget() { return m_widget; } + void loadConfiguration(IUAVGadgetConfiguration* config); + +private: + PfdQmlGadgetWidget *m_widget; +}; + + +#endif // PFDQMLQMLGADGET_H_ diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index 992b4acd2..a1653cf53 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -223,6 +223,13 @@ plugin_uavobjectwidgetutils.depends += plugin_uavobjectutil plugin_uavobjectwidgetutils.depends += plugin_uavsettingsimportexport SUBDIRS += plugin_uavobjectwidgetutils +# Setup Wizard plugin +plugin_setupwizard.subdir = setupwizard +plugin_setupwizard.depends = plugin_coreplugin +plugin_setupwizard.depends += plugin_uavobjects +plugin_setupwizard.depends += plugin_config +SUBDIRS += plugin_setupwizard + # Junsi Powerlog plugin #plugin_powerlog.subdir = powerlog #plugin_powerlog.depends = plugin_coreplugin diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp index 57b17c193..c916b9cc5 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp @@ -165,7 +165,7 @@ bool ChronoPlotData::append(UAVObject* obj) //Perform scope math, if necessary if (mathFunction == "Boxcar average" || mathFunction == "Standard deviation"){ - //Put the new value at the front + //Put the new value at the back yDataHistory->append( currentValue ); // calculate average value diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp index 7b53a060f..9798df0e7 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp @@ -677,13 +677,13 @@ int ScopeGadgetWidget::csvLoggingAddData() } else { - ss << QString().sprintf("%3.6g",plotData2->yData->last()); + ss << QString().sprintf("%3.10g",plotData2->yData->last()); m_csvLoggingDataValid=1; } } else { - ss << QString().sprintf("%3.6g",plotData2->yDataHistory->last()); + ss << QString().sprintf("%3.10g",plotData2->yData->last()); m_csvLoggingDataValid=1; } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/SetupWizard.pluginspec b/ground/openpilotgcs/src/plugins/setupwizard/SetupWizard.pluginspec new file mode 100644 index 000000000..8aa610ec8 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/SetupWizard.pluginspec @@ -0,0 +1,12 @@ + + The OpenPilot Project + (C) 2012 OpenPilot Project + The GNU Public License (GPL) Version 3 + A plugin that provides a setup wizard for easy initial setup of airframes. + http://www.openpilot.org + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp new file mode 100644 index 000000000..1b94c1cae --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -0,0 +1,193 @@ +/** + ****************************************************************************** + * + * @file connectiondiagram.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup ConnectionDiagram + * @{ + * @brief + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include +#include "connectiondiagram.h" +#include "ui_connectiondiagram.h" + +ConnectionDiagram::ConnectionDiagram(QWidget *parent, VehicleConfigurationSource* configSource) : + QDialog(parent), ui(new Ui::ConnectionDiagram), m_configSource(configSource), m_background(0) +{ + ui->setupUi(this); + setWindowTitle(tr("Connection Diagram")); + setupGraphicsScene(); +} + +ConnectionDiagram::~ConnectionDiagram() +{ + delete ui; +} + +void ConnectionDiagram::resizeEvent(QResizeEvent *event) +{ + QWidget::resizeEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); +} + +void ConnectionDiagram::showEvent(QShowEvent * event) +{ + QWidget::showEvent(event); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); +} + +void ConnectionDiagram::setupGraphicsScene() +{ + m_renderer = new QSvgRenderer(); + if (QFile::exists(QString(":/setupwizard/resources/connection-diagrams.svg")) && + m_renderer->load(QString(":/setupwizard/resources/connection-diagrams.svg")) && + m_renderer->isValid()) + { + m_scene = new QGraphicsScene(this); + ui->connectionDiagram->setScene(m_scene); + //ui->connectionDiagram->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + + m_background = new QGraphicsSvgItem(); + m_background->setSharedRenderer(m_renderer); + m_background->setElementId("background"); + m_background->setOpacity(0); + //m_background->setFlags(QGraphicsItem::ItemClipsToShape); + m_background->setZValue(-1); + m_scene->addItem(m_background); + + QList elementsToShow; + + switch(m_configSource->getControllerType()) + { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + case VehicleConfigurationSource::CONTROLLER_REVO: + case VehicleConfigurationSource::CONTROLLER_PIPX: + default: + elementsToShow << "controller"; + break; + } + + switch (m_configSource->getVehicleType()) + { + case VehicleConfigurationSource::VEHICLE_MULTI: + switch (m_configSource->getVehicleSubType()) + { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + elementsToShow << "tri"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + elementsToShow << "quad-x"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + elementsToShow << "quad-p"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + elementsToShow << "hexa"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + elementsToShow << "hexa-y"; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + elementsToShow << "hexa-h"; + break; + default: + break; + } + break; + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + default: + break; + } + + switch (m_configSource->getInputType()) + { + case VehicleConfigurationSource::INPUT_PWM: + elementsToShow << "pwm" ; + break; + case VehicleConfigurationSource::INPUT_PPM: + elementsToShow << "ppm"; + break; + case VehicleConfigurationSource::INPUT_SBUS: + elementsToShow << "sbus"; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + elementsToShow << "satellite"; + break; + default: + break; + } + + setupGraphicsSceneItems(elementsToShow); + + ui->connectionDiagram->setSceneRect(m_background->boundingRect()); + ui->connectionDiagram->fitInView(m_background, Qt::KeepAspectRatio); + + qDebug() << "Scene complete"; + } +} + +void ConnectionDiagram::setupGraphicsSceneItems(QList elementsToShow) +{ + qreal z = 0; + QRectF backgBounds = m_renderer->boundsOnElement("background"); + + foreach(QString elementId, elementsToShow) { + if(m_renderer->elementExists(elementId)) { + QGraphicsSvgItem* element = new QGraphicsSvgItem(); + element->setSharedRenderer(m_renderer); + element->setElementId(elementId); + element->setZValue(z++); + element->setOpacity(1.0); + + QMatrix matrix = m_renderer->matrixForElement(elementId); + QRectF orig = matrix.mapRect(m_renderer->boundsOnElement(elementId)); + element->setPos(orig.x(),orig.y()); + + //QRectF orig = m_renderer->boundsOnElement(elementId); + //element->setPos(orig.x() - backgBounds.x(), orig.y() - backgBounds.y()); + + m_scene->addItem(element); + qDebug() << "Adding " << elementId << " to scene at " << element->pos(); + } + else { + qDebug() << "Element with id: " << elementId << " not found."; + } + } +} + +void ConnectionDiagram::on_saveButton_clicked() +{ + QImage image(2200, 1100, QImage::Format_ARGB32); + image.fill(0); + QPainter painter(&image); + m_scene->render(&painter); + QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "", tr("Images (*.png *.xpm *.jpg)")); + if(!fileName.isEmpty()) { + image.save(fileName); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h new file mode 100644 index 000000000..e03ed6b0e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.h @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * + * @file connectiondiagram.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup ConnectionDiagram + * @{ + * @brief + *****************************************************************************/ +/* + * 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 CONNECTIONDIAGRAM_H +#define CONNECTIONDIAGRAM_H + +#include +#include +#include + +#include +#include "vehicleconfigurationsource.h" + + +namespace Ui { +class ConnectionDiagram; +} + +class ConnectionDiagram : public QDialog +{ + Q_OBJECT + +public: + explicit ConnectionDiagram(QWidget *parent, VehicleConfigurationSource *configSource); + ~ConnectionDiagram(); + +private: + Ui::ConnectionDiagram *ui; + VehicleConfigurationSource *m_configSource; + + QSvgRenderer *m_renderer; + QGraphicsSvgItem* m_background; + QGraphicsScene *m_scene; + + void setupGraphicsScene(); + void setupGraphicsSceneItems(QList elementsToShow); +protected: + void resizeEvent(QResizeEvent *event); + void showEvent(QShowEvent *event); + +private slots: + + void on_saveButton_clicked(); +}; + +#endif // CONNECTIONDIAGRAM_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.ui b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.ui new file mode 100644 index 000000000..7e86a99a2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.ui @@ -0,0 +1,112 @@ + + + ConnectionDiagram + + + + 0 + 0 + 800 + 440 + + + + Dialog + + + true + + + true + + + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + 255 + 255 + 255 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + Save + + + + + + + + 0 + 0 + + + + Close + + + true + + + + + + + + + + + closeButton + clicked() + ConnectionDiagram + close() + + + 752 + 418 + + + 399 + 219 + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp new file mode 100644 index 000000000..23d422adf --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp @@ -0,0 +1,217 @@ +/** + ****************************************************************************** + * + * @file levellingutil.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup LevellingUtil + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "levellingutil.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "attitudesettings.h" +#include "accels.h" +#include "gyros.h" + + +LevellingUtil::LevellingUtil(long measurementCount, long measurementRate) : QObject(), + m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount), + m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate) +{ +} + +LevellingUtil::LevellingUtil(long accelMeasurementCount, long accelMeasurementRate, + long gyroMeasurementCount, long gyroMeasurementRate) : QObject(), + m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount), + m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate) +{ +} + +void LevellingUtil::start() +{ + if(!m_isMeasuring) { + startMeasurement(); + + // Set up timeout timer + connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); + //Set timeout to max time of measurement + 10 secs + m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000); + } +} + +void LevellingUtil::abort() +{ + if(m_isMeasuring) { + stopMeasurement(); + } +} + +void LevellingUtil::gyroMeasurementsUpdated(UAVObject *obj) +{ + QMutexLocker locker(&m_measurementMutex); + + if(m_receivedGyroUpdates < m_gyroMeasurementCount) { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); + + Gyros * gyros = Gyros::GetInstance(uavObjectManager); + Gyros::DataFields gyrosData = gyros->getData(); + + m_gyroX += gyrosData.x; + m_gyroY += gyrosData.y; + m_gyroZ += gyrosData.z; + + m_receivedGyroUpdates++; + emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); + } + else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + stopMeasurement(); + emit done(calculateLevellingData()); + } +} + +void LevellingUtil::accelMeasurementsUpdated(UAVObject *obj) +{ + QMutexLocker locker(&m_measurementMutex); + + if(m_receivedAccelUpdates < m_accelMeasurementCount) { + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); + + Accels * accels = Accels::GetInstance(uavObjectManager); + Accels::DataFields accelsData = accels->getData(); + + m_accelerometerX += accelsData.x; + m_accelerometerY += accelsData.y; + m_accelerometerZ += accelsData.z; + + m_receivedAccelUpdates++; + emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount); + } + else if (m_receivedAccelUpdates >= m_accelMeasurementCount && + m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) { + stopMeasurement(); + emit done(calculateLevellingData()); + } +} + +void LevellingUtil::timeout() +{ + QMutexLocker locker(&m_measurementMutex); + + stopMeasurement(); + emit timeout(tr("Calibration timed out before receiving required updates.")); +} + +void LevellingUtil::startMeasurement() +{ + QMutexLocker locker(&m_measurementMutex); + + m_isMeasuring = true; + + // Reset variables + m_receivedAccelUpdates = 0; + m_accelerometerX = 0; + m_accelerometerY = 0; + m_accelerometerZ = 0; + + m_receivedGyroUpdates = 0; + m_gyroX = 0; + m_gyroY = 0; + m_gyroZ = 0; + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); + + // Disable gyro bias correction to see raw data + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); + attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; + AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); + + // Set up to receive updates for accels + UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); + connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + + // Set update period for accels + m_previousAccelMetaData = uavObject->getMetadata(); + UAVObject::Metadata newMetaData = m_previousAccelMetaData; + UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC); + newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate; + uavObject->setMetadata(newMetaData); + + // Set up to receive updates from gyros + uavObject = Gyros::GetInstance(uavObjectManager); + connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + + // Set update period for gyros + m_previousGyroMetaData = uavObject->getMetadata(); + newMetaData = m_previousGyroMetaData; + UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC); + newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate; + uavObject->setMetadata(newMetaData); +} + +void LevellingUtil::stopMeasurement() +{ + m_isMeasuring = false; + + //Stop timeout timer + m_timeoutTimer.stop(); + disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout())); + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); + + // Stop listening for updates from accels + UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*))); + uavObject->setMetadata(m_previousAccelMetaData); + + // Stop listening for updates from gyros + uavObject = Gyros::GetInstance(uavObjectManager); + disconnect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*))); + uavObject->setMetadata(m_previousGyroMetaData); + + // Enable gyro bias correction again + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); + attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; + AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); +} + +accelGyroBias LevellingUtil::calculateLevellingData() +{ + accelGyroBias bias; + bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; + bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates / ACCELERATION_SCALE; + bias.m_accelerometerZBias = (m_accelerometerZ / (double)m_receivedAccelUpdates + G) / ACCELERATION_SCALE; + + bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates * 100.0; + bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates * 100.0; + bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates * 100.0; + return bias; +} + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h new file mode 100644 index 000000000..c5cbe8709 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h @@ -0,0 +1,92 @@ +/** + ****************************************************************************** + * + * @file levellingutil.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup LevellingUtil + * @{ + * @brief + *****************************************************************************/ +/* + * 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 LEVELLINGUTIL_H +#define LEVELLINGUTIL_H + +#include +#include +#include + +#include "uavobject.h" +#include "vehicleconfigurationsource.h" + +class LevellingUtil : public QObject +{ + Q_OBJECT +public: + explicit LevellingUtil(long measurementCount, long measurementRate); + explicit LevellingUtil(long accelMeasurementCount, long accelMeasurementRate, + long gyroMeasurementCount, long gyroMeasurementRate); + +signals: + void progress(long current, long total); + void done(accelGyroBias measuredBias); + void timeout(QString message); + +public slots: + void start(); + void abort(); + +private slots: + void gyroMeasurementsUpdated(UAVObject * obj); + void accelMeasurementsUpdated(UAVObject * obj); + void timeout(); + +private: + static const float G = 9.81f; + static const float ACCELERATION_SCALE = 0.004f * 9.81f; + + QMutex m_measurementMutex; + QTimer m_timeoutTimer; + + bool m_isMeasuring; + long m_receivedAccelUpdates; + long m_receivedGyroUpdates; + + long m_accelMeasurementCount; + long m_gyroMeasurementCount; + long m_accelMeasurementRate; + long m_gyroMeasurementRate; + + UAVObject::Metadata m_previousGyroMetaData; + UAVObject::Metadata m_previousAccelMetaData; + + double m_accelerometerX; + double m_accelerometerY; + double m_accelerometerZ; + double m_gyroX; + double m_gyroY; + double m_gyroZ; + + void stop(); + void startMeasurement(); + void stopMeasurement(); + accelGyroBias calculateLevellingData(); +}; + +#endif // LEVELLINGUTIL_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp new file mode 100644 index 000000000..30144ec42 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * + * @file outputcalibrationutil.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputCalibrationUtil + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "outputcalibrationutil.h" +#include "actuatorcommand.h" +#include "extensionsystem/pluginmanager.h" +#include "vehicleconfigurationhelper.h" +#include "manualcontrolsettings.h" + +OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) : + QObject(parent), m_outputChannel(-1), m_safeValue(1000) +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_uavObjectManager = pm->getObject(); + Q_ASSERT(m_uavObjectManager); +} + +OutputCalibrationUtil::~OutputCalibrationUtil() +{ + stopChannelOutput(); +} + +void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValue) +{ + if(m_outputChannel < 0 && channel >= 0 && channel < ActuatorCommand::CHANNEL_NUMELEM) + { + //Start output... + m_outputChannel = channel; + m_safeValue = safeValue; + + qDebug() << "Starting output for channel " << m_outputChannel << "..."; + + ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); + Q_ASSERT(actuatorCommand); + UAVObject::Metadata metaData = actuatorCommand->getMetadata(); + m_savedActuatorCommandMetadata = metaData; + + //Store current data for later restore + m_savedActuatorCommandData = actuatorCommand->getData(); + + //Enable actuator control from GCS... + //Store current metadata for later restore + UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY); + UAVObject::SetFlightTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); + UAVObject::SetGcsTelemetryAcked(metaData, false); + UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE); + metaData.gcsTelemetryUpdatePeriod = 100; + + //Apply changes + actuatorCommand->setMetadata(metaData); + actuatorCommand->updated(); + + qDebug() << "Output for channel " << m_outputChannel << " started."; + } +} + +void OutputCalibrationUtil::stopChannelOutput() +{ + if(m_outputChannel >= 0) + { + qDebug() << "Stopping output for channel " << m_outputChannel << "..."; + //Stop output... + setChannelOutputValue(m_safeValue); + qDebug() << "Settings output for channel " << m_outputChannel << " to " << m_safeValue; + + // Restore metadata to what it was before + ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); + Q_ASSERT(actuatorCommand); + //actuatorCommand->setData(m_savedActuatorCommandData); + actuatorCommand->setMetadata(m_savedActuatorCommandMetadata); + actuatorCommand->updated(); + + qDebug() << "Output for channel " << m_outputChannel << " stopped."; + + m_outputChannel = -1; + } +} + +void OutputCalibrationUtil::setChannelOutputValue(quint16 value) +{ + if(m_outputChannel >= 0) + { + //Set output value + qDebug() << "Setting output value for channel " << m_outputChannel << " to " << value << "."; + ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); + Q_ASSERT(actuatorCommand); + ActuatorCommand::DataFields data = actuatorCommand->getData(); + data.Channel[m_outputChannel] = value; + actuatorCommand->setData(data); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h new file mode 100644 index 000000000..dc9fc7fc8 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h @@ -0,0 +1,62 @@ +/** + ****************************************************************************** + * + * @file outputcalibrationutil.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputCalibrationUtil + * @{ + * @brief + *****************************************************************************/ +/* + * 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 OUTPUTCALIBRATIONUTIL_H +#define OUTPUTCALIBRATIONUTIL_H + +#include +#include +#include "uavobject.h" +#include "uavobjectmanager.h" +#include "vehicleconfigurationsource.h" +#include "actuatorcommand.h" + + +class OutputCalibrationUtil : public QObject +{ + Q_OBJECT +public: + explicit OutputCalibrationUtil(QObject *parent = 0); + ~OutputCalibrationUtil(); + +signals: + +public slots: + void startChannelOutput(quint16 channel, quint16 safeValue); + void stopChannelOutput(); + + void setChannelOutputValue(quint16 value); + +private: + qint16 m_outputChannel; + quint16 m_safeValue; + UAVObject::Metadata m_savedActuatorCommandMetadata; + ActuatorCommand::DataFields m_savedActuatorCommandData; + UAVObjectManager *m_uavObjectManager; +}; + +#endif // OUTPUTCALIBRATIONUTIL_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp new file mode 100644 index 000000000..31425fc34 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.cpp @@ -0,0 +1,35 @@ +/** + ****************************************************************************** + * + * @file abstractwizardpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup AbstractWizardPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "abstractwizardpage.h" + +AbstractWizardPage::AbstractWizardPage(SetupWizard* wizard, QWidget *parent) : + QWizardPage(parent) +{ + m_wizard = wizard; + setFixedSize(600, 400); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h new file mode 100644 index 000000000..072d619e7 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/abstractwizardpage.h @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * + * @file abstractwizardpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup AbstractWizardPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 ABSTRACTWIZARDPAGE_H +#define ABSTRACTWIZARDPAGE_H + +#include +#include "setupwizard.h" + +class AbstractWizardPage : public QWizardPage +{ + Q_OBJECT +protected: + explicit AbstractWizardPage(SetupWizard *wizard, QWidget *parent = 0); + +private: + SetupWizard *m_wizard; + +public: + SetupWizard* getWizard() { return m_wizard; } + +}; + +#endif // ABSTRACTWIZARDPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp new file mode 100644 index 000000000..deed9d6eb --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.cpp @@ -0,0 +1,212 @@ +/** + ****************************************************************************** + * + * @file controllerpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup ControllerPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "controllerpage.h" +#include "ui_controllerpage.h" +#include "setupwizard.h" + +#include +#include + +ControllerPage::ControllerPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::ControllerPage) +{ + ui->setupUi(this); + + m_connectionManager = getWizard()->getConnectionManager(); + Q_ASSERT(m_connectionManager); + connect(m_connectionManager, SIGNAL(availableDevicesChanged(QLinkedList)), this, SLOT(devicesChanged(QLinkedList))); + + ExtensionSystem::PluginManager *pluginManager = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pluginManager); + m_telemtryManager = pluginManager->getObject(); + Q_ASSERT(m_telemtryManager); + connect(m_telemtryManager, SIGNAL(connected()), this, SLOT(connectionStatusChanged())); + connect(m_telemtryManager, SIGNAL(disconnected()), this, SLOT(connectionStatusChanged())); + + connect(ui->connectButton, SIGNAL(clicked()), this, SLOT(connectDisconnect())); + + setupBoardTypes(); + setupDeviceList(); +} + +ControllerPage::~ControllerPage() +{ + delete ui; +} + +void ControllerPage::initializePage() +{ + if(anyControllerConnected()) { + SetupWizard::CONTROLLER_TYPE type = getControllerType(); + setControllerType(type); + } + else { + setControllerType(SetupWizard::CONTROLLER_UNKNOWN); + } + emit completeChanged(); +} + +bool ControllerPage::isComplete() const +{ + return m_telemtryManager->isConnected() && ui->boardTypeCombo->currentIndex() > 0 && + m_connectionManager->getCurrentDevice().getConName().startsWith("USB:", Qt::CaseInsensitive); +} + +bool ControllerPage::validatePage() +{ + getWizard()->setControllerType((SetupWizard::CONTROLLER_TYPE)ui->boardTypeCombo->itemData(ui->boardTypeCombo->currentIndex()).toInt()); + return true; +} + +bool ControllerPage::anyControllerConnected() +{ + return m_telemtryManager->isConnected(); +} + +SetupWizard::CONTROLLER_TYPE ControllerPage::getControllerType() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectUtilManager* utilMngr = pm->getObject(); + int id = utilMngr->getBoardModel(); + + switch (id) { + case 0x0301: + return SetupWizard::CONTROLLER_PIPX; + case 0x0401: + return SetupWizard::CONTROLLER_CC; + case 0x0402: + return SetupWizard::CONTROLLER_CC3D; + case 0x0901: + return SetupWizard::CONTROLLER_REVO; + default: + return SetupWizard::CONTROLLER_UNKNOWN; + } +} + +void ControllerPage::setupDeviceList() +{ + devicesChanged(m_connectionManager->getAvailableDevices()); + connectionStatusChanged(); +} + +void ControllerPage::setupBoardTypes() +{ + QVariant v(0); + ui->boardTypeCombo->addItem(tr(""), SetupWizard::CONTROLLER_UNKNOWN); + ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl"), SetupWizard::CONTROLLER_CC); + ui->boardTypeCombo->addItem(tr("OpenPilot CopterControl 3D"), SetupWizard::CONTROLLER_CC3D); + ui->boardTypeCombo->addItem(tr("OpenPilot Revolution"), SetupWizard::CONTROLLER_REVO); + //ui->boardTypeCombo->model()->setData(ui->boardTypeCombo->model()->index(ui->boardTypeCombo->count() - 1, 0), v, Qt::UserRole - 1); + ui->boardTypeCombo->addItem(tr("OpenPilot PipX Radio Modem"), SetupWizard::CONTROLLER_PIPX); + //ui->boardTypeCombo->model()->setData(ui->boardTypeCombo->model()->index(ui->boardTypeCombo->count() - 1, 0), v, Qt::UserRole - 1); +} + +void ControllerPage::setControllerType(SetupWizard::CONTROLLER_TYPE type) +{ + for(int i = 0; i < ui->boardTypeCombo->count(); ++i) { + if(ui->boardTypeCombo->itemData(i) == type) { + ui->boardTypeCombo->setCurrentIndex(i); + break; + } + } +} + +void ControllerPage::devicesChanged(QLinkedList devices) +{ + // Get the selected item before the update if any + QString currSelectedDeviceName = ui->deviceCombo->currentIndex() != -1 ? + ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString() : ""; + + // Clear the box + ui->deviceCombo->clear(); + + int indexOfSelectedItem = -1; + int i = 0; + + // Loop and fill the combo with items from connectionmanager + foreach (Core::DevListItem deviceItem, devices) + { + ui->deviceCombo->addItem(deviceItem.getConName()); + QString deviceName = (const QString)deviceItem.getConName(); + ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, deviceName, Qt::ToolTipRole); + if(!deviceName.startsWith("USB:", Qt::CaseInsensitive)) { + ui->deviceCombo->setItemData(ui->deviceCombo->count() - 1, QVariant(0), Qt::UserRole - 1); + } + if(currSelectedDeviceName != "" && currSelectedDeviceName == deviceName) { + indexOfSelectedItem = i; + } + i++; + } + + // Re select the item that was selected before if any + if(indexOfSelectedItem != -1) { + ui->deviceCombo->setCurrentIndex(indexOfSelectedItem); + } + //connectionStatusChanged(); +} + +void ControllerPage::connectionStatusChanged() +{ + if(m_connectionManager->isConnected()) { + ui->deviceCombo->setEnabled(false); + ui->connectButton->setText(tr("Disconnect")); + ui->boardTypeCombo->setEnabled(false); + QString connectedDeviceName = m_connectionManager->getCurrentDevice().getConName(); + for(int i = 0; i < ui->deviceCombo->count(); ++i) { + if(connectedDeviceName == ui->deviceCombo->itemData(i, Qt::ToolTipRole).toString()) { + ui->deviceCombo->setCurrentIndex(i); + break; + } + } + + SetupWizard::CONTROLLER_TYPE type = getControllerType(); + setControllerType(type); + qDebug() << "Connection status changed: Connected, controller type: " << getControllerType(); + } + else { + ui->deviceCombo->setEnabled(true); + ui->connectButton->setText(tr("Connect")); + ui->boardTypeCombo->setEnabled(false); + ui->boardTypeCombo->model()->setData(ui->boardTypeCombo->model()->index(0, 0), QVariant(0), Qt::UserRole - 1); + setControllerType(SetupWizard::CONTROLLER_UNKNOWN); + qDebug() << "Connection status changed: Disconnected"; + } + emit completeChanged(); +} + +void ControllerPage::connectDisconnect() +{ + if(m_connectionManager->isConnected()) { + m_connectionManager->disconnectDevice(); + } + else { + m_connectionManager->connectDevice(m_connectionManager->findDevice(ui->deviceCombo->itemData(ui->deviceCombo->currentIndex(), Qt::ToolTipRole).toString())); + } + emit completeChanged(); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h new file mode 100644 index 000000000..9370f4cd4 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.h @@ -0,0 +1,68 @@ +/** + ****************************************************************************** + * + * @file controllerpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup ControllerPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 CONTROLLERPAGE_H +#define CONTROLLERPAGE_H + +#include +#include +#include "setupwizard.h" +#include "uavtalk/telemetrymanager.h" +#include "abstractwizardpage.h" + +namespace Ui { +class ControllerPage; +} + +class ControllerPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit ControllerPage(SetupWizard *wizard, QWidget *parent = 0); + ~ControllerPage(); + void initializePage(); + bool isComplete() const; + bool validatePage(); + +private: + Ui::ControllerPage *ui; + bool anyControllerConnected(); + SetupWizard::CONTROLLER_TYPE getControllerType(); + void setupDeviceList(); + void setupBoardTypes(); + void setControllerType(SetupWizard::CONTROLLER_TYPE type); + Core::ConnectionManager *m_connectionManager; + TelemetryManager *m_telemtryManager; + +private slots: + void devicesChanged(QLinkedList devices); + void connectionStatusChanged(); + void connectDisconnect(); +}; + +#endif // CONTROLLERPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui new file mode 100644 index 000000000..885feda4c --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui @@ -0,0 +1,127 @@ + + + ControllerPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 550 + 261 + + + + <!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: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-size:12pt; font-weight:600;">OpenPilot board identification</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;"><br /></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-size:10pt;">To continue, the wizard needs to determine the configuration required for the type of OpenPilot controller you have. When connected, the wizard will attempt to automatically detect the type of board.</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-size:10pt;"><br /></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-size:10pt;">If the board is already connected and successfully detected, the board type will already be displayed. You can </span><span style=" font-size:10pt; font-weight:600;">Disconnect</span><span style=" font-size:10pt;"> and select another device if you need to detect another board.</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-size:10pt;"><br /></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-size:10pt;">If your board is not connected, please connect the board to a USB port on your computer and select the device from the list below. Then press </span><span style=" font-size:10pt; font-weight:600;">Connect</span><span style=" font-size:10pt;">.</span></p></body></html> + + + Qt::AutoText + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 460 + 350 + 100 + 23 + + + + Connect + + + + + + 40 + 350 + 125 + 16 + + + + + 10 + 50 + false + + + + Detected board type: + + + + + false + + + + 170 + 350 + 279 + 22 + + + + + + + 170 + 310 + 279 + 22 + + + + + + + 40 + 310 + 121 + 16 + + + + + 10 + 50 + false + + + + Connection device: + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp new file mode 100644 index 000000000..34228c401 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.cpp @@ -0,0 +1,65 @@ +/** + ****************************************************************************** + * + * @file endpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Setup Wizard Plugin + * @{ + * @brief A Wizard to make the initial setup easy for everyone. + *****************************************************************************/ +/* + * 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 "endpage.h" +#include "ui_endpage.h" +#include +#include +#include +#include + +EndPage::EndPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::EndPage) +{ + ui->setupUi(this); + setFinalPage(true); + connect(ui->inputWizardButton, SIGNAL(clicked()), this, SLOT(openInputWizard())); +} + +EndPage::~EndPage() +{ + delete ui; +} + +void EndPage::openInputWizard() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + ConfigGadgetFactory* configGadgetFactory = pm->getObject(); + + if(configGadgetFactory) { + //Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration"); + getWizard()->close(); + configGadgetFactory->startInputWizard(); + } + else { + QMessageBox msgBox; + msgBox.setText(tr("Unable to open Input Wizard since the Config Plugin is not\nloaded in the current workspace.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h new file mode 100644 index 000000000..dfbad7942 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * + * @file endpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup + * @{ + * @brief + *****************************************************************************/ +/* + * 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 ENDPAGE_H +#define ENDPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class EndPage; +} + +class EndPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit EndPage(SetupWizard *wizard, QWidget *parent = 0); + ~EndPage(); + +private slots: + void openInputWizard(); + +private: + Ui::EndPage *ui; +}; + +#endif // ENDPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.ui new file mode 100644 index 000000000..b8d9f104c --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.ui @@ -0,0 +1,88 @@ + + + EndPage + + + + 0 + 0 + 600 + 400 + + + + + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 550 + 251 + + + + <!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: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-size:12pt; font-weight:600;">Congratulations!</span></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-size:12pt; font-weight:600;">Setup wizard is completed.</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-size:12pt; font-weight:600;"><br /></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-size:10pt;">This part of the setup procedure is now complete and you are one step away from completing the setup of your OpenPilot controller.</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-size:10pt;"><br /></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-size:10pt;">To complete the setup please click the Radio Setup Wizard button below to close this wizard and go directly to the Radio Setup Wizard.</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-size:10pt;"><br /></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 200 + 270 + 200 + 100 + + + + QToolButton { border: none } + + + Go to Input Wizard... + + + + :/setupwizard/resources/bttn-txwizard-off.png + :/setupwizard/resources/bttn-txwizard-on.png + :/setupwizard/resources/bttn-txwizard-off.png + :/setupwizard/resources/bttn-txwizard-off.png:/setupwizard/resources/bttn-txwizard-off.png + + + + 200 + 100 + + + + true + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp new file mode 100644 index 000000000..7295cc8a3 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * + * @file fixedwingpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup FixedWingPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "fixedwingpage.h" +#include "ui_fixedwingpage.h" + +FixedWingPage::FixedWingPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::FixedWingPage) +{ + ui->setupUi(this); + setFinalPage(true); +} + +FixedWingPage::~FixedWingPage() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h new file mode 100644 index 000000000..697721a9e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file fixedwingpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup FixedWingPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 FIXEDWINGPAGE_H +#define FIXEDWINGPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class FixedWingPage; +} + +class FixedWingPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit FixedWingPage(SetupWizard *wizard, QWidget *parent = 0); + ~FixedWingPage(); + +private: + Ui::FixedWingPage *ui; +}; + +#endif // FIXEDWINGPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui new file mode 100644 index 000000000..ed0e963e9 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui @@ -0,0 +1,43 @@ + + + FixedWingPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 50 + 160 + 500 + 41 + + + + <!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-size:12pt; font-weight:600;">The Fixed Wing section of the OpenPilot Setup Wizard is not yet implemented</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-size:8pt;"></p></body></html> + + + Qt::AlignHCenter|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.cpp new file mode 100644 index 000000000..93bfc3503 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.cpp @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * + * @file helipage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup HeliPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "helipage.h" +#include "ui_helipage.h" + +HeliPage::HeliPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::HeliPage) +{ + ui->setupUi(this); + setFinalPage(true); +} + +HeliPage::~HeliPage() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h new file mode 100644 index 000000000..2b2b5b894 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file helipage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup HeliPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 HELIPAGE_H +#define HELIPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class HeliPage; +} + +class HeliPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit HeliPage(SetupWizard *wizard, QWidget *parent = 0); + ~HeliPage(); + +private: + Ui::HeliPage *ui; +}; + +#endif // HELIPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.ui new file mode 100644 index 000000000..8e14881d9 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/helipage.ui @@ -0,0 +1,43 @@ + + + HeliPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 50 + 160 + 500 + 41 + + + + <!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-size:12pt; font-weight:600;">The Helicopter section of the OpenPilot Setup Wizard is not yet implemented</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-size:8pt;"></p></body></html> + + + Qt::AlignHCenter|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp new file mode 100644 index 000000000..607df838a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -0,0 +1,91 @@ +/** + ****************************************************************************** + * + * @file inputpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup InputPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "inputpage.h" +#include "ui_inputpage.h" +#include "setupwizard.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "hwsettings.h" + +InputPage::InputPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + + ui(new Ui::InputPage) +{ + ui->setupUi(this); +} + +InputPage::~InputPage() +{ + delete ui; +} + +bool InputPage::validatePage() +{ + if(ui->pwmButton->isChecked()) { + getWizard()->setInputType(SetupWizard::INPUT_PWM); + } + else if(ui->ppmButton->isChecked()) { + getWizard()->setInputType(SetupWizard::INPUT_PPM); + } + else if(ui->sbusButton->isChecked()) { + getWizard()->setInputType(SetupWizard::INPUT_SBUS); + } + else if(ui->spectrumButton->isChecked()) { + getWizard()->setInputType(SetupWizard::INPUT_DSM2); + } + else { + getWizard()->setInputType(SetupWizard::INPUT_PWM); + } + getWizard()->setRestartNeeded(getWizard()->isRestartNeeded() || restartNeeded(getWizard()->getInputType())); + + return true; +} + +bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedType) +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *uavoManager = pm->getObject(); + Q_ASSERT(uavoManager); + HwSettings* hwSettings = HwSettings::GetInstance(uavoManager); + HwSettings::DataFields data = hwSettings->getData(); + switch(selectedType) + { + case VehicleConfigurationSource::INPUT_PWM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PWM; + case VehicleConfigurationSource::INPUT_PPM: + return data.CC_RcvrPort != HwSettings::CC_RCVRPORT_PPM; + case VehicleConfigurationSource::INPUT_SBUS: + return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; + case VehicleConfigurationSource::INPUT_DSM2: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; + default: + return true; + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h new file mode 100644 index 000000000..dde50b6e6 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.h @@ -0,0 +1,51 @@ +/** + ****************************************************************************** + * + * @file inputpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup InputPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 INPUTPAGE_H +#define INPUTPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class InputPage; +} + +class InputPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit InputPage(SetupWizard *wizard, QWidget *parent = 0); + ~InputPage(); + bool validatePage(); + +private: + bool restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedType); + Ui::InputPage *ui; +}; + +#endif // INPUTPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui new file mode 100644 index 000000000..af4129982 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui @@ -0,0 +1,239 @@ + + + InputPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 221 + + + + <!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: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-size:12pt; font-weight:600;">OpenPilot basic input signal 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-size:12pt; font-weight:600;"><br /></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-size:10pt;">The OpenPilot controller supports many different types of input signals. Please select the type of input that matches your receiver configuration. If you are unsure, just leave the default option selected and continue the wizard.</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-size:10pt;"><br /></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-size:10pt;">Some input options require the OpenPilot controller to be rebooted before the changes can take place. If an option that requires a reboot is selected, you will be instructed to do so on the next page of this wizard.</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 430 + 250 + 100 + 120 + + + + + 10 + + + + Spectrum Satellite + + + QToolButton { border: none } + + + Spectrum + + + + :/setupwizard/resources/bttn-sat-up.png + :/setupwizard/resources/bttn-sat-down.png:/setupwizard/resources/bttn-sat-up.png + + + + 100 + 100 + + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + 70 + 250 + 100 + 120 + + + + + 10 + + + + PWM - One cable per channel + + + QToolButton { border: none } + + + PWM + + + + :/setupwizard/resources/bttn-pwm-up.png + :/setupwizard/resources/bttn-pwm-down.png:/setupwizard/resources/bttn-pwm-up.png + + + + 100 + 100 + + + + true + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + 190 + 250 + 100 + 120 + + + + + 10 + + + + PPM - One cable for all channels + + + QToolButton { border: none } + + + PPM + + + + :/setupwizard/resources/bttn-ppm-up.png + :/setupwizard/resources/bttn-ppm-down.png:/setupwizard/resources/bttn-ppm-up.png + + + + 100 + 100 + + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + 310 + 250 + 100 + 120 + + + + + 10 + + + + Futaba S-BUS + + + QToolButton { border: none } + + + Futaba + + + + :/setupwizard/resources/bttn-sbus-up.png + :/setupwizard/resources/bttn-sbus-down.png:/setupwizard/resources/bttn-sbus-up.png + + + + 100 + 100 + + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp new file mode 100644 index 000000000..28dac7c33 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp @@ -0,0 +1,136 @@ +/** + ****************************************************************************** + * + * @file levellingpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup LevellingPage + * @{ + * @brief + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include "levellingpage.h" +#include "ui_levellingpage.h" +#include "setupwizard.h" + +LevellingPage::LevellingPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::LevellingPage), m_levellingUtil(0) +{ + ui->setupUi(this); + connect(ui->levelButton, SIGNAL(clicked()), this, SLOT(performLevelling())); +} + +LevellingPage::~LevellingPage() +{ + if(m_levellingUtil) { + delete m_levellingUtil; + } + delete ui; +} + +bool LevellingPage::validatePage() +{ + return true; +} + +bool LevellingPage::isComplete() const +{ + //const_cast(this)->getWizard()->isLevellingPerformed() && + return ui->levelButton->isEnabled(); +} + +void LevellingPage::enableButtons(bool enable) +{ + ui->levelButton->setEnabled(enable); + getWizard()->button(QWizard::NextButton)->setEnabled(enable); + getWizard()->button(QWizard::CancelButton)->setEnabled(enable); + getWizard()->button(QWizard::BackButton)->setEnabled(enable); + QApplication::processEvents(); +} + +void LevellingPage::performLevelling() +{ + if(!getWizard()->getConnectionManager()->isConnected()) { + QMessageBox msgBox; + msgBox.setText(tr("An OpenPilot controller must be connected to your computer to perform bias " + "calculations.\nPlease connect your OpenPilot controller to your computer and try again.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + return; + } + + enableButtons(false); + ui->progressLabel->setText(QString(tr("Retrieving data..."))); + + + if(!m_levellingUtil) + { + m_levellingUtil = new LevellingUtil(BIAS_CYCLES, BIAS_RATE); + } + + connect(m_levellingUtil, SIGNAL(progress(long,long)), this, SLOT(levellingProgress(long,long))); + connect(m_levellingUtil, SIGNAL(done(accelGyroBias)), this, SLOT(levellingDone(accelGyroBias))); + connect(m_levellingUtil, SIGNAL(timeout(QString)), this, SLOT(levellingTimeout(QString))); + + m_levellingUtil->start(); +} + +void LevellingPage::levellingProgress(long current, long total) +{ + if(ui->levellinProgressBar->maximum() != (int)total) { + ui->levellinProgressBar->setMaximum((int)total); + } + if(ui->levellinProgressBar->value() != (int)current) { + ui->levellinProgressBar->setValue((int)current); + } +} + +void LevellingPage::levellingDone(accelGyroBias bias) +{ + stopLevelling(); + getWizard()->setLevellingBias(bias); + emit completeChanged(); +} + +void LevellingPage::levellingTimeout(QString message) +{ + stopLevelling(); + + QMessageBox msgBox; + msgBox.setText(message); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); +} + +void LevellingPage::stopLevelling() +{ + if(m_levellingUtil) + { + disconnect(m_levellingUtil, SIGNAL(progress(long,long)), this, SLOT(levellingProgress(long,long))); + disconnect(m_levellingUtil, SIGNAL(done(accelGyroBias)), this, SLOT(levellingDone(accelGyroBias))); + disconnect(m_levellingUtil, SIGNAL(timeout(QString)), this, SLOT(levellingTimeout(QString))); + ui->progressLabel->setText(QString(tr("Done!"))); + enableButtons(true); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h new file mode 100644 index 000000000..06b18ed75 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.h @@ -0,0 +1,65 @@ +/** + ****************************************************************************** + * + * @file levellingpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup LevellingPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 LEVELLINGPAGE_H +#define LEVELLINGPAGE_H + +#include "abstractwizardpage.h" +#include "levellingutil.h" + +namespace Ui { +class LevellingPage; +} + +class LevellingPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit LevellingPage(SetupWizard *wizard, QWidget *parent = 0); + ~LevellingPage(); + bool validatePage(); + bool isComplete() const; + +private slots: + void performLevelling(); + void levellingProgress(long current, long total); + void levellingDone(accelGyroBias bias); + void levellingTimeout(QString message); + +private: + static const int BIAS_CYCLES = 200; + static const int BIAS_RATE = 30; + + Ui::LevellingPage *ui; + LevellingUtil *m_levellingUtil; + + void stopLevelling(); + void enableButtons(bool enable); +}; + +#endif // LEVELLINGPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.ui new file mode 100644 index 000000000..dae71e6b7 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.ui @@ -0,0 +1,124 @@ + + + LevellingPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 241 + + + + <!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;"> +<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:'MS Shell Dlg 2'; font-size:12pt; font-weight:600;">OpenPilot controller leveling calibration procedure</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:'MS Shell Dlg 2'; font-size:12pt; 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:'MS Shell Dlg 2'; font-size:10pt;">The wizard needs to get information from the controller to determine in which position the vehicle is normally considered to be level. To be able to successfully perform these measurements, you need to place the vehicle on a surface that is as flat and level as possible. Examples of such surfaces could be a table top or the floor. Be careful to ensure that the vehicle really is level, since this step will affect the accelerometer and gyro bias in the controller software.</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:'MS Shell Dlg 2'; font-size:10pt;"></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:'MS Shell Dlg 2'; font-size:10pt;">To perform the leveling, please push the Calculate button and wait for the process to finish. </span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 200 + 260 + 200 + 70 + + + + Calculate gyro and accelerometer bias + + + QToolButton { border: none } + + + Calculate + + + + :/setupwizard/resources/bttn-calculate-up.png + :/setupwizard/resources/bttn-calculate-down.png:/setupwizard/resources/bttn-calculate-up.png + + + + 200 + 70 + + + + + + + 40 + 350 + 520 + 23 + + + + QProgressBar { + border: 2px solid grey; + border-radius: 5px; + text-align: center; + } +QProgressBar::chunk { + background-color: #3D6699; + width: 10px; + margin: 0.5px; + } + + + 0 + + + + + + + + + 42 + 330 + 511 + 16 + + + + + 10 + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp new file mode 100644 index 000000000..26076f39e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp @@ -0,0 +1,171 @@ +/** + ****************************************************************************** + * + * @file multipage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup MultiPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "multipage.h" +#include "ui_multipage.h" +#include "setupwizard.h" + +MultiPage::MultiPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::MultiPage) +{ + ui->setupUi(this); + + QSvgRenderer *renderer = new QSvgRenderer(); + renderer->load(QString(":/configgadget/images/multirotor-shapes.svg")); + multiPic = new QGraphicsSvgItem(); + multiPic->setSharedRenderer(renderer); + QGraphicsScene *scene = new QGraphicsScene(this); + scene->addItem(multiPic); + ui->typeGraphicsView->setScene(scene); + + setupMultiTypesCombo(); + + // Default to Quad X since it is the most common setup + ui->typeCombo->setCurrentIndex(1); + connect(ui->typeCombo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateImageAndDescription())); +} + +MultiPage::~MultiPage() +{ + delete ui; +} + +void MultiPage::initializePage() +{ + updateAvailableTypes(); + updateImageAndDescription(); +} + +bool MultiPage::validatePage() +{ + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + getWizard()->setVehicleSubType(type); + return true; +} + +void MultiPage::setupMultiTypesCombo() +{ + ui->typeCombo->addItem(tr("Tricopter"), SetupWizard::MULTI_ROTOR_TRI_Y); + m_descriptions << tr("The Tricopter uses three motors and one servo. The servo is used to give yaw authority to the rear motor. " + "The front motors are rotating in opposite directions. The Tricopter is known for its sweeping yaw movement and " + "it is very well suited for FPV since the front rotors are spread wide apart."); + + ui->typeCombo->addItem(tr("Quadcopter X"), SetupWizard::MULTI_ROTOR_QUAD_X); + m_descriptions << tr("The X Quadcopter uses four motors and is the most common multi rotor configuration. Two of the motors rotate clockwise " + "and two counter clockwise. The motors positioned diagonal to each other rotate in the same direction. " + "This setup is perfect for sport flying and is also commonly used for FPV platforms."); + + ui->typeCombo->addItem(tr("Quadcopter +"), SetupWizard::MULTI_ROTOR_QUAD_PLUS); + m_descriptions << tr("The Plus(+) Quadcopter uses four motors and is similar to the X Quadcopter but the forward direction is offset by 45 degrees. " + "The motors front and rear rotate in clockwise and the motors right and left rotate counter-clockwise. " + "This setup was one of the first to be used and is still used for sport flying. This configuration is not that well suited " + "for FPV since the fore rotor tend to be in the way of the camera."); + + ui->typeCombo->addItem(tr("Hexacopter"), SetupWizard::MULTI_ROTOR_HEXA); + m_descriptions << tr("Hexacopter"); + + ui->typeCombo->addItem(tr("Hexacopter Coax (Y6)"), SetupWizard::MULTI_ROTOR_HEXA_COAX_Y); + m_descriptions << tr("Hexacopter Coax (Y6)"); + + ui->typeCombo->addItem(tr("Hexacopter X"), SetupWizard::MULTI_ROTOR_HEXA_H); + m_descriptions << tr("Hexacopter H"); + + // Fredrik Arvidsson(m_thread) 2012-08-26 Disable Octos until further notice + /* + ui->typeCombo->addItem(tr("Octocopter"), SetupWizard::MULTI_ROTOR_OCTO); + m_descriptions << tr("Octocopter"); + + ui->typeCombo->addItem(tr("Octocopter Coax X"), SetupWizard::MULTI_ROTOR_OCTO_COAX_X); + m_descriptions << tr("Octocopter Coax X"); + + ui->typeCombo->addItem(tr("Octocopter Coax +"), SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS); + m_descriptions << tr("Octocopter Coax +"); + + ui->typeCombo->addItem(tr("Octocopter V"), SetupWizard::MULTI_ROTOR_OCTO_V); + m_descriptions << tr("Octocopter V"); + */ +} + +void MultiPage::updateAvailableTypes() +{ + /* + QVariant enable = (getWizard()->getInputType() == SetupWizard::INPUT_PWM) ? QVariant(0) : QVariant(1 | 32); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(6, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(7, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(8, 0), enable, Qt::UserRole - 1); + ui->typeCombo->model()->setData(ui->typeCombo->model()->index(9, 0), enable, Qt::UserRole - 1); + */ +} + +void MultiPage::updateImageAndDescription() +{ + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE) ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + QString elementId = ""; + QString description = m_descriptions.at(ui->typeCombo->currentIndex()); + switch(type) + { + case SetupWizard::MULTI_ROTOR_TRI_Y: + elementId = "tri"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + elementId = "quad-x"; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + elementId = "quad-plus"; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + elementId = "quad-hexa"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + elementId = "hexa-coax"; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + elementId = "quad-hexa-H"; + break; + case SetupWizard::MULTI_ROTOR_OCTO: + elementId = "quad-octo"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + elementId = "octo-coax-X"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + elementId = "octo-coax-P"; + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + elementId = "quad-octo-v"; + break; + default: + elementId = ""; + break; + } + multiPic->setElementId(elementId); + ui->typeGraphicsView->setSceneRect(multiPic->boundingRect()); + ui->typeGraphicsView->fitInView(multiPic, Qt::KeepAspectRatio); + + ui->typeDescription->setText(description); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h new file mode 100644 index 000000000..a4d8ebf42 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.h @@ -0,0 +1,64 @@ +/** + ****************************************************************************** + * + * @file multipage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup MultiPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 MULTIPAGE_H +#define MULTIPAGE_H + +#include +#include +#include + +#include "abstractwizardpage.h" + +namespace Ui { +class MultiPage; +} + +class MultiPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit MultiPage(SetupWizard *wizard, QWidget *parent = 0); + ~MultiPage(); + + void initializePage(); + bool validatePage(); + + +private: + Ui::MultiPage *ui; + void setupMultiTypesCombo(); + QGraphicsSvgItem *multiPic; + void updateAvailableTypes(); + QList m_descriptions; + +private slots: + void updateImageAndDescription(); +}; + +#endif // MULTIPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui new file mode 100644 index 000000000..22e244836 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui @@ -0,0 +1,150 @@ + + + MultiPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 151 + + + + <!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: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-size:12pt; font-weight:600;">OpenPilot multirotor 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-size:12pt; font-weight:600;"><br /></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-size:10pt;">This part of the wizard will set up the OpenPilot controller for use with a flying platform utilizing multiple rotors. The wizard supports the most common types of multirotors. Other variants of multirotors can be configured by using custom configuration options in the Configuration plugin in the GCS.</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-size:10pt;"><br /></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-size:10pt;">Please select the type of multirotor you want to create a configuration for below:</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 390 + 200 + 170 + 170 + + + + + 0 + 0 + + + + false + + + QFrame::NoFrame + + + 0 + + + 0 + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + 0 + 0 + 0 + + + + + + + 0 + 0 + 0 + + + + + false + + + + + + 40 + 200 + 90 + 16 + + + + + 10 + 50 + false + + + + Multirotor type: + + + + + + 140 + 198 + 230 + 22 + + + + + + + 40 + 240 + 330 + 130 + + + + Qt::ScrollBarAlwaysOn + + + Qt::ScrollBarAlwaysOff + + + Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.cpp new file mode 100644 index 000000000..57a43fda8 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.cpp @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * + * @file notyetimplementedpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup NotYetImplementedPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "notyetimplementedpage.h" +#include "ui_notyetimplementedpage.h" + +NotYetImplementedPage::NotYetImplementedPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::NotYetImplementedPage) +{ + ui->setupUi(this); + setFinalPage(true); +} + +NotYetImplementedPage::~NotYetImplementedPage() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h new file mode 100644 index 000000000..4f323d9fc --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file notyetimplementedpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup NotYetImplementedPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 NOTYETIMPLEMENTEDPAGE_H +#define NOTYETIMPLEMENTEDPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class NotYetImplementedPage; +} + +class NotYetImplementedPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit NotYetImplementedPage(SetupWizard *wizard, QWidget *parent = 0); + ~NotYetImplementedPage(); + +private: + Ui::NotYetImplementedPage *ui; +}; + +#endif // NOTYETIMPLEMENTEDPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.ui new file mode 100644 index 000000000..776a53540 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/notyetimplementedpage.ui @@ -0,0 +1,43 @@ + + + NotYetImplementedPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 50 + 190 + 501 + 31 + + + + <!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-size:12pt; font-weight:600;">This section of the OpenPilot Setup Wizard is not yet implemented</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-size:8pt;"></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp new file mode 100644 index 000000000..d91dddc81 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -0,0 +1,388 @@ +/** + ****************************************************************************** + * + * @file outputcalibrationpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputCalibrationPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "outputcalibrationpage.h" +#include "ui_outputcalibrationpage.h" +#include "systemalarms.h" +#include "uavobjectmanager.h" + +OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), ui(new Ui::OutputCalibrationPage), m_vehicleBoundsItem(0), + m_currentWizardIndex(-1), m_calibrationUtil(0) +{ + ui->setupUi(this); + + m_vehicleRenderer = new QSvgRenderer(); + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) + { + m_vehicleScene = new QGraphicsScene(this); + ui->vehicleView->setScene(m_vehicleScene); + } +} + +OutputCalibrationPage::~OutputCalibrationPage() +{ + if(m_calibrationUtil) { + delete m_calibrationUtil; + m_calibrationUtil = 0; + } + delete ui; +} + +void OutputCalibrationPage::setupVehicle() +{ + m_actuatorSettings = getWizard()->getActuatorSettings(); + m_wizardIndexes.clear(); + m_vehicleElementIds.clear(); + m_vehicleHighlightElementIndexes.clear(); + m_channelIndex.clear(); + m_currentWizardIndex = 0; + m_vehicleScene->clear(); + switch(getWizard()->getVehicleSubType()) + { + case SetupWizard::MULTI_ROTOR_TRI_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; + m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; + m_actuatorSettings[3].channelMin = 1500; + m_actuatorSettings[3].channelNeutral = 1500; + m_actuatorSettings[3].channelMax = 1500; + getWizard()->setActuatorSettings(m_actuatorSettings); + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; + m_channelIndex << 0 << 0 << 1 << 2 << 3; + break; + case SetupWizard::MULTI_ROTOR_HEXA: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; + m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + default: + break; + } + + VehicleConfigurationHelper helper(getWizard()); + helper.setupVehicle(false); + + if(m_calibrationUtil) { + delete m_calibrationUtil; + m_calibrationUtil = 0; + } + m_calibrationUtil = new OutputCalibrationUtil(); + + setupVehicleItems(); +} + +void OutputCalibrationPage::setupVehicleItems() +{ + m_vehicleItems.clear(); + m_vehicleBoundsItem = new QGraphicsSvgItem(); + m_vehicleBoundsItem->setSharedRenderer(m_vehicleRenderer); + m_vehicleBoundsItem->setElementId(m_vehicleElementIds[0]); + m_vehicleBoundsItem->setZValue(-1); + m_vehicleBoundsItem->setOpacity(0); + m_vehicleScene->addItem(m_vehicleBoundsItem); + + QRectF parentBounds = m_vehicleRenderer->boundsOnElement(m_vehicleElementIds[0]); + + for(int i = 1; i < m_vehicleElementIds.size(); i++) + { + QGraphicsSvgItem *item = new QGraphicsSvgItem(); + item->setSharedRenderer(m_vehicleRenderer); + item->setElementId(m_vehicleElementIds[i]); + item->setZValue(i); + item->setOpacity(1.0); + + QRectF itemBounds = m_vehicleRenderer->boundsOnElement(m_vehicleElementIds[i]); + item->setPos(itemBounds.x() - parentBounds.x(), itemBounds.y() - parentBounds.y()); + + m_vehicleScene->addItem(item); + m_vehicleItems << item; + } +} + +void OutputCalibrationPage::startWizard() +{ + ui->calibrationStack->setCurrentIndex(m_wizardIndexes[0]); + setupVehicleHighlightedPart(); +} + +void OutputCalibrationPage::setupVehicleHighlightedPart() +{ + qreal dimOpaque = m_currentWizardIndex == 0 ? 1.0 : 0.3; + qreal highlightOpaque = 1.0; + int highlightedIndex = m_vehicleHighlightElementIndexes[m_currentWizardIndex]; + for(int i = 0; i < m_vehicleItems.size(); i++) { + QGraphicsSvgItem* item = m_vehicleItems[i]; + item->setOpacity((highlightedIndex == i) ? highlightOpaque : dimOpaque); + } +} + +void OutputCalibrationPage::setWizardPage() +{ + qDebug() << "Wizard index: " << m_currentWizardIndex; + m_calibrationUtil->stopChannelOutput(); + + QApplication::processEvents(); + + int currentPageIndex = m_wizardIndexes[m_currentWizardIndex]; + qDebug() << "Current page: " << currentPageIndex; + ui->calibrationStack->setCurrentIndex(currentPageIndex); + + int currentChannel = getCurrentChannel(); + qDebug() << "Current channel: " << currentChannel; + if(currentChannel >= 0) { + if(currentPageIndex == 1) { + ui->motorNeutralSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); + } + else if(currentPageIndex == 2) { + ui->servoCenterSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); + } + else if(currentPageIndex == 3) { + ui->servoMinAngleSlider->setMaximum(m_actuatorSettings[currentChannel].channelNeutral); + ui->servoMinAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMin); + } + else if(currentPageIndex == 4) { + ui->servoMaxAngleSlider->setMinimum(m_actuatorSettings[currentChannel].channelNeutral); + ui->servoMaxAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMax); + } + } + setupVehicleHighlightedPart(); +} + +void OutputCalibrationPage::initializePage() +{ + if(m_vehicleScene) { + setupVehicle(); + startWizard(); + } +} + +bool OutputCalibrationPage::validatePage() +{ + if(isFinished()) { + getWizard()->setActuatorSettings(m_actuatorSettings); + return true; + } else { + m_currentWizardIndex++; + setWizardPage(); + return false; + } +} + +void OutputCalibrationPage::showEvent(QShowEvent *event) +{ + Q_UNUSED(event); + if(m_vehicleBoundsItem) { + ui->vehicleView->setSceneRect(m_vehicleBoundsItem->boundingRect()); + ui->vehicleView->fitInView(m_vehicleBoundsItem, Qt::KeepAspectRatio); + } +} + +void OutputCalibrationPage::customBackClicked() +{ + if(m_currentWizardIndex > 0) + { + m_currentWizardIndex--; + setWizardPage(); + } + else + { + getWizard()->back(); + } +} + +quint16 OutputCalibrationPage::getCurrentChannel() +{ + return m_channelIndex[m_currentWizardIndex]; +} + +void OutputCalibrationPage::enableButtons(bool enable) +{ + getWizard()->button(QWizard::NextButton)->setEnabled(enable); + getWizard()->button(QWizard::CustomButton1)->setEnabled(enable); + getWizard()->button(QWizard::CancelButton)->setEnabled(enable); + getWizard()->button(QWizard::BackButton)->setEnabled(enable); + QApplication::processEvents(); +} + +void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked) +{ + ui->motorNeutralButton->setText(checked ? tr("Stop") : tr("Start")); + quint16 channel = getCurrentChannel(); + onStartButtonToggle(ui->motorNeutralButton, channel, m_actuatorSettings[channel].channelNeutral, 1000, ui->motorNeutralSlider); +} + +void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) { + if(button->isChecked()) { + if(checkAlarms()) { + enableButtons(false); + m_calibrationUtil->startChannelOutput(channel, safeValue); + slider->setValue(value); + m_calibrationUtil->setChannelOutputValue(value); + } + else { + button->setChecked(false); + } + } + else { + m_calibrationUtil->stopChannelOutput(); + enableButtons(true); + } + debugLogChannelValues(); +} + +bool OutputCalibrationPage::checkAlarms() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *uavObjectManager = pm->getObject(); + Q_ASSERT(uavObjectManager); + SystemAlarms * systemAlarms = SystemAlarms::GetInstance(uavObjectManager); + Q_ASSERT(systemAlarms); + SystemAlarms::DataFields data = systemAlarms->getData(); + + if(data.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { + QMessageBox mbox; + mbox.setText(QString(tr("The actuator module is in an error state.\n\n" + "This error can be caused by not having the board correctly connected or\n" + "if the board is not sufficiently powered by an external power source like\n" + "a battery. To use only USB as power source is not enough when the USB can't\n" + "power external components like ESCs and servos.\n\n" + "Please fix the error before continuing calibration."))); + mbox.setStandardButtons(QMessageBox::Ok); + mbox.setIcon(QMessageBox::Critical); + mbox.exec(); + return false; + } + return true; +} + +void OutputCalibrationPage::debugLogChannelValues() +{ + quint16 channel = getCurrentChannel(); + qDebug() << "ChannelMin : " << m_actuatorSettings[channel].channelMin; + qDebug() << "ChannelNeutral: " << m_actuatorSettings[channel].channelNeutral; + qDebug() << "ChannelMax : " << m_actuatorSettings[channel].channelMax; +} + +void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value) +{ + if(ui->motorNeutralButton->isChecked()) { + quint16 value = ui->motorNeutralSlider->value(); + m_calibrationUtil->setChannelOutputValue(value); + m_actuatorSettings[getCurrentChannel()].channelNeutral = value; + debugLogChannelValues(); + } +} + +void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked) +{ + ui->servoCenterButton->setText(checked ? tr("Stop") : tr("Start")); + quint16 channel = getCurrentChannel(); + quint16 safeValue = m_actuatorSettings[channel].channelNeutral; + onStartButtonToggle(ui->servoCenterButton, channel, safeValue, safeValue, ui->servoCenterSlider); +} + +void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position) +{ + if(ui->servoCenterButton->isChecked()) { + quint16 value = ui->servoCenterSlider->value(); + m_calibrationUtil->setChannelOutputValue(value); + quint16 channel = getCurrentChannel(); + m_actuatorSettings[channel].channelNeutral = value; + + //Adjust min and max + if(value < m_actuatorSettings[channel].channelMin) { + m_actuatorSettings[channel].channelMin = value; + } + if(value > m_actuatorSettings[channel].channelMax) { + m_actuatorSettings[channel].channelMax = value; + } + debugLogChannelValues(); + } +} + +void OutputCalibrationPage::on_servoMinAngleButton_toggled(bool checked) +{ + ui->servoMinAngleButton->setText(checked ? tr("Stop") : tr("Start")); + quint16 channel = getCurrentChannel(); + quint16 safeValue = m_actuatorSettings[channel].channelNeutral; + onStartButtonToggle(ui->servoMinAngleButton, channel, m_actuatorSettings[channel].channelMin, safeValue, ui->servoMinAngleSlider); +} + +void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position) +{ + if(ui->servoMinAngleButton->isChecked()) { + quint16 value = ui->servoMinAngleSlider->value(); + m_calibrationUtil->setChannelOutputValue(value); + m_actuatorSettings[getCurrentChannel()].channelMin = value; + debugLogChannelValues(); + } +} + +void OutputCalibrationPage::on_servoMaxAngleButton_toggled(bool checked) +{ + ui->servoMaxAngleButton->setText(checked ? tr("Stop") : tr("Start")); + quint16 channel = getCurrentChannel(); + quint16 safeValue = m_actuatorSettings[channel].channelNeutral; + onStartButtonToggle(ui->servoMaxAngleButton, channel, m_actuatorSettings[channel].channelMax, safeValue, ui->servoMaxAngleSlider); +} + +void OutputCalibrationPage::on_servoMaxAngleSlider_valueChanged(int position) +{ + if(ui->servoMaxAngleButton->isChecked()) { + quint16 value = ui->servoMaxAngleSlider->value(); + m_calibrationUtil->setChannelOutputValue(value); + m_actuatorSettings[getCurrentChannel()].channelMax = value; + debugLogChannelValues(); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h new file mode 100644 index 000000000..76af9534e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h @@ -0,0 +1,103 @@ +/** + ****************************************************************************** + * + * @file outputcalibrationpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputCalibrationPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 OUTPUTCALIBRATIONPAGE_H +#define OUTPUTCALIBRATIONPAGE_H + +#include +#include "abstractwizardpage.h" +#include "setupwizard.h" +#include "outputcalibrationutil.h" +#include "vehicleconfigurationhelper.h" + +namespace Ui { +class OutputCalibrationPage; +} + +class OutputCalibrationPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit OutputCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); + ~OutputCalibrationPage(); + void initializePage(); + bool validatePage(); + + bool isFinished() { return m_currentWizardIndex >= m_wizardIndexes.size() - 1; } + +protected: + void showEvent(QShowEvent *event); + +public slots: + void customBackClicked(); + +private slots: + void on_motorNeutralButton_toggled(bool checked); + void on_motorNeutralSlider_valueChanged(int value); + + void on_servoCenterButton_toggled(bool checked); + void on_servoCenterSlider_valueChanged(int position); + + void on_servoMinAngleButton_toggled(bool checked); + void on_servoMinAngleSlider_valueChanged(int position); + + void on_servoMaxAngleButton_toggled(bool checked); + void on_servoMaxAngleSlider_valueChanged(int position); + +private: + void setupVehicle(); + void startWizard(); + void setupVehicleItems(); + void setupVehicleHighlightedPart(); + void setWizardPage(); + void enableButtons(bool enable); + void onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider); + bool checkAlarms(); + void debugLogChannelValues(); + quint16 getCurrentChannel(); + + Ui::OutputCalibrationPage *ui; + QSvgRenderer *m_vehicleRenderer; + QGraphicsScene *m_vehicleScene; + QGraphicsSvgItem *m_vehicleBoundsItem; + + qint16 m_currentWizardIndex; + + QList m_vehicleElementIds; + QList m_vehicleItems; + QList m_vehicleHighlightElementIndexes; + QList m_channelIndex; + QList m_wizardIndexes; + + QList m_actuatorSettings; + + OutputCalibrationUtil *m_calibrationUtil; + +}; + +#endif // OUTPUTCALIBRATIONPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui new file mode 100644 index 000000000..6b6d8810f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui @@ -0,0 +1,497 @@ + + + OutputCalibrationPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 310 + 40 + 270 + 341 + + + + QFrame::NoFrame + + + + + + 20 + 40 + 290 + 341 + + + + 0 + + + + + + 0 + 10 + 270 + 321 + + + + <!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:8pt; font-weight:400; font-style:normal;"> +<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:10pt;">It is now time to calibrate the output levels for the signals controlling your vehicle. </span></p> +<p align="center" 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:10pt; font-weight:600; color:#ff0000;">VERY IMPORTANT!</span><span style=" font-family:'Lucida Grande'; font-size:10pt;"><br /></span><span style=" font-family:'Lucida Grande'; font-size:10pt; font-weight:600; color:#ff0000;">REMOVE ALL PROPELLERS FROM THE VEHICLE BEFORE PROCEEDING!</span></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:'Lucida Grande'; font-size:10pt;">Connect all components according to the illustration on the summary page, and provide power using an external power supply such as a battery before continuing.</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:'Lucida Grande'; font-size:10pt;"><br /></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:'Lucida Grande'; font-size:10pt;">Depending on what vehicle you have selected, both the motors controlled by ESCs and/or servos controlled directly by the OpenPilot controller may have to be calibrated. The following steps will guide you safely through this process. </span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + + + 0 + 10 + 261 + 221 + + + + <html><head/><body><p>In this step we will set the neutral rate for the motor highlighted in the illustration to the right. <br/>Plase pay attention to the details and in particular the motors position and its rotation direction.</p><p>To find the neutral rate for this engine, press the Start button below and slide the slider to the right until the engine just starts to spin stably. <br/><br/>When done press button again to stop.</p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + false + + + + 10 + 250 + 241 + 19 + + + + 1000 + + + 1300 + + + 10 + + + 20 + + + Qt::Horizontal + + + false + + + QSlider::TicksBelow + + + 20 + + + + + + 90 + 290 + 75 + 23 + + + + Start + + + true + + + false + + + + + + + + 0 + 10 + 261 + 201 + + + + <html><head/><body><p>This step calibrates the center position of the servo. To set the center position for this servo, press the Start button below and slide the slider to center the servo. </p><p>When done press button again to stop.</p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + false + + + + 10 + 250 + 241 + 19 + + + + 1000 + + + 2000 + + + 10 + + + 20 + + + 1500 + + + true + + + Qt::Horizontal + + + false + + + false + + + QSlider::TicksBelow + + + 40 + + + + + + 90 + 290 + 75 + 23 + + + + Start + + + true + + + false + + + + + + + + 90 + 290 + 75 + 23 + + + + Start + + + true + + + false + + + + + + 0 + 10 + 261 + 211 + + + + <html><head/><body><p>To save the servo and other hardware from damage we have to set the max and min angles for the servo. </p><p>To set the minimum angle for the servo, press the Start button below and select the top slider and slide it to the left until min angle is reached.</p><p>When done press button again to stop.</p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + false + + + + 10 + 250 + 241 + 20 + + + + 1000 + + + 1500 + + + 10 + + + 20 + + + 1500 + + + true + + + Qt::Horizontal + + + false + + + false + + + QSlider::TicksBelow + + + 40 + + + + + + + + 90 + 290 + 75 + 23 + + + + Start + + + true + + + false + + + + + false + + + + 10 + 250 + 241 + 20 + + + + 1500 + + + 2000 + + + 10 + + + 20 + + + 1500 + + + true + + + Qt::Horizontal + + + false + + + false + + + QSlider::TicksBelow + + + 40 + + + + + + 0 + 10 + 261 + 211 + + + + <html><head/><body><p>To set the maximum angle for the servo, press the Start button below and select the top slider and slide it to the right until max angle is reached.</p><p>When done press button again to stop.</p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + + + 20 + 20 + 531 + 16 + + + + + 12 + 75 + true + + + + Output calibration + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + motorNeutralButton + toggled(bool) + motorNeutralSlider + setEnabled(bool) + + + 147 + 291 + + + 150 + 249 + + + + + servoMinAngleButton + toggled(bool) + servoMinAngleSlider + setEnabled(bool) + + + 147 + 291 + + + 150 + 249 + + + + + servoMaxAngleButton + toggled(bool) + servoMaxAngleSlider + setEnabled(bool) + + + 147 + 291 + + + 150 + 249 + + + + + servoCenterButton + toggled(bool) + servoCenterSlider + setEnabled(bool) + + + 147 + 291 + + + 150 + 249 + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp new file mode 100644 index 000000000..d9903af57 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp @@ -0,0 +1,55 @@ +/** + ****************************************************************************** + * + * @file outputpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "outputpage.h" +#include "ui_outputpage.h" +#include "setupwizard.h" + +OutputPage::OutputPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + + ui(new Ui::OutputPage) +{ + ui->setupUi(this); +} + +OutputPage::~OutputPage() +{ + delete ui; +} + +bool OutputPage::validatePage() +{ + if(ui->rapidESCButton->isChecked()) { + getWizard()->setESCType(SetupWizard::ESC_RAPID); + } + else { + getWizard()->setESCType(SetupWizard::ESC_LEGACY); + } + + return true; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h new file mode 100644 index 000000000..3a7130238 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.h @@ -0,0 +1,50 @@ +/** + ****************************************************************************** + * + * @file outputpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 OUTPUTPAGE_H +#define OUTPUTPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class OutputPage; +} + +class OutputPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit OutputPage(SetupWizard *wizard, QWidget *parent = 0); + ~OutputPage(); + bool validatePage(); + +private: + Ui::OutputPage *ui; +}; + +#endif // OUTPUTPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui new file mode 100644 index 000000000..a89c1fc77 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui @@ -0,0 +1,150 @@ + + + OutputPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 151 + + + + <!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;"> +<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:'MS Shell Dlg 2'; font-size:12pt; font-weight:600;">OpenPilot basic output signal 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:'MS Shell Dlg 2'; font-size:12pt; 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:'MS Shell Dlg 2'; font-size:10pt;">To set an optimal configuration of the output signals powering your motors, the wizard needs to know what type of Electronic Speed Controllers (ESCs) you will use and what their capabilities are.</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:'MS Shell Dlg 2'; font-size:10pt;"></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:'MS Shell Dlg 2'; font-size:10pt;">Please select one of the options below. If you are unsure about the capabilities of your ESCs, just leave the default option selected and continue the wizard.</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:'MS Shell Dlg 2'; font-size:10pt;"></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:'MS Shell Dlg 2'; font-size:10pt;">To read more regarding ESC refresh rates, please see </span><a href="http://wiki.openpilot.org/display/Doc/TurboPWM+ESC%27s"><span style=" text-decoration: underline; color:#0000ff;">this article</span></a><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;"> in the OpenPilot Wiki</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 320 + 250 + 200 + 120 + + + + + 10 + + + + Turbo PWM ESC 400Hz + + + QToolButton { border: none } + + + Turbo PWM + + + + :/setupwizard/resources/bttn-turbo-up.png + :/setupwizard/resources/bttn-turbo-down.png:/setupwizard/resources/bttn-turbo-up.png + + + + 200 + 100 + + + + true + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + 80 + 250 + 200 + 120 + + + + + 10 + + + + Standard ESC 50Hz + + + QToolButton { border: none } + + + Standard ESC + + + + :/setupwizard/resources/bttn-ESC-up.png + :/setupwizard/resources/bttn-ESC-down.png:/setupwizard/resources/bttn-ESC-up.png + + + + 200 + 100 + + + + true + + + false + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp new file mode 100644 index 000000000..43153380b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.cpp @@ -0,0 +1,67 @@ +/** + ****************************************************************************** + * + * @file rebootpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup RebootPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "rebootpage.h" +#include "ui_rebootpage.h" + +RebootPage::RebootPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::RebootPage), m_toggl(false) +{ + ui->setupUi(this); + ui->yellowLabel->setVisible(false); + ui->redLabel->setVisible(true); +} + +RebootPage::~RebootPage() +{ + disconnect(&m_timer, SIGNAL(timeout()), this, SLOT(toggleLabel())); + m_timer.stop(); + delete ui; +} + +void RebootPage::initializePage() +{ + if(!m_timer.isActive()) { + connect(&m_timer, SIGNAL(timeout()), this, SLOT(toggleLabel())); + m_timer.setInterval(500); + m_timer.setSingleShot(false); + m_timer.start(); + } +} + +bool RebootPage::validatePage() +{ + return true; +} + +void RebootPage::toggleLabel() +{ + m_toggl = !m_toggl; + ui->yellowLabel->setVisible(m_toggl); + ui->redLabel->setVisible(!m_toggl); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h new file mode 100644 index 000000000..c3d32fca2 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.h @@ -0,0 +1,57 @@ +/** + ****************************************************************************** + * + * @file rebootpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup RebootPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 REBOOTPAGE_H +#define REBOOTPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class RebootPage; +} + +class RebootPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit RebootPage(SetupWizard *wizard, QWidget *parent = 0); + ~RebootPage(); + + void initializePage(); + bool validatePage(); + +private: + Ui::RebootPage *ui; + QTimer m_timer; + bool m_toggl; + +private slots: + void toggleLabel(); +}; + +#endif // REBOOTPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.ui new file mode 100644 index 000000000..46a0402cc --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/rebootpage.ui @@ -0,0 +1,91 @@ + + + RebootPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 50 + 130 + 501 + 41 + + + + <!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:8pt; font-weight:400; font-style:normal;"> +<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-size:18pt; color:#ff0000;">PLEASE REBOOT YOUR CONTROLLER</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 50 + 180 + 501 + 151 + + + + <!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:8pt; font-weight:400; font-style:normal;"> +<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-size:10pt; color:#000000;">The configuration created by the wizard contains settings that require a reboot of your controller. Please power cycle the controller before continuing. To power cycle the controller remove all batteries and the USB cable for at least 30 seconds. </span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 50 + 130 + 501 + 41 + + + + <!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:8pt; font-weight:400; font-style:normal;"> +<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-size:18pt; color:#ffd500;">PLEASE REBOOT YOUR CONTROLLER</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + yellowLabel + redLabel + label_3 + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp new file mode 100644 index 000000000..f3e03e0fb --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.cpp @@ -0,0 +1,103 @@ +/** + ****************************************************************************** + * + * @file savepage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SavePage + * @{ + * @brief + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include "savepage.h" +#include "ui_savepage.h" +#include "setupwizard.h" +#include "vehicleconfigurationhelper.h" + +SavePage::SavePage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::SavePage), m_successfulWrite(false) +{ + ui->setupUi(this); + connect(ui->saveButton, SIGNAL(clicked()), this, SLOT(writeToController())); +} + +SavePage::~SavePage() +{ + delete ui; +} + +bool SavePage::validatePage() +{ + return true; +} + +bool SavePage::isComplete() const +{ + return m_successfulWrite; +} + +void SavePage::writeToController() +{ + if(!getWizard()->getConnectionManager()->isConnected()) { + QMessageBox msgBox; + msgBox.setText(tr("An OpenPilot controller must be connected to your computer to save the " + "configuration.\nPlease connect your OpenPilot controller to your computer and try again.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + return; + } + + enableButtons(false); + VehicleConfigurationHelper helper(getWizard()); + connect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + + m_successfulWrite = helper.setupVehicle(); + + disconnect(&helper, SIGNAL(saveProgress(int, int, QString)),this, SLOT(saveProgress(int, int, QString))); + ui->saveProgressLabel->setText(QString("%2").arg(m_successfulWrite ? "green" : "red", ui->saveProgressLabel->text())); + enableButtons(true); + + emit completeChanged(); +} + +void SavePage::enableButtons(bool enable) +{ + ui->saveButton->setEnabled(enable); + getWizard()->button(QWizard::NextButton)->setEnabled(enable); + getWizard()->button(QWizard::CancelButton)->setEnabled(enable); + getWizard()->button(QWizard::BackButton)->setEnabled(enable); + QApplication::processEvents(); +} + +void SavePage::saveProgress(int total, int current, QString description) +{ + qDebug() << "Progress " << current << "(" << total << ")"; + if(ui->saveProgressBar->maximum() != total) { + ui->saveProgressBar->setMaximum(total); + } + if(ui->saveProgressBar->value() != current) { + ui->saveProgressBar->setValue(current); + } + if(ui->saveProgressLabel->text() != description) { + ui->saveProgressLabel->setText(description); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h new file mode 100644 index 000000000..d8382df93 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.h @@ -0,0 +1,57 @@ +/** + ****************************************************************************** + * + * @file savepage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SavePage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 SAVEPAGE_H +#define SAVEPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class SavePage; +} + +class SavePage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit SavePage(SetupWizard *wizard, QWidget *parent = 0); + ~SavePage(); + bool validatePage(); + bool isComplete() const; + +private: + Ui::SavePage *ui; + bool m_successfulWrite; + void enableButtons(bool enable); + +private slots: + void writeToController(); + void saveProgress(int total, int current, QString description); +}; + +#endif // SAVEPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.ui new file mode 100644 index 000000000..d5a5710ed --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/savepage.ui @@ -0,0 +1,132 @@ + + + SavePage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 231 + + + + <!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: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-size:12pt; font-weight:600;">OpenPilot configuration ready to save</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-size:12pt; font-weight:600;"><br /></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-size:10pt;">The wizard is now ready to save the configuration directly to your OpenPilot controller. </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-size:10pt;"><br /></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-size:10pt;"><br /></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-size:10pt;">If any of the selections made in this wizard require a reboot of the controller, then power cycling the OpenPilot controller board will have to be performed after you save in this step.</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-size:10pt;"><br /></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-size:10pt;">Press the Save button to save the 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-size:10pt;"><br /></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-size:10pt;"><br /></span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 225 + 260 + 150 + 70 + + + + Write configuration to controller + + + QToolButton { border: none } + + + Save + + + + :/setupwizard/resources/bttn-save-up.png + :/setupwizard/resources/bttn-save-down.png:/setupwizard/resources/bttn-save-up.png + + + + 200 + 70 + + + + Qt::ToolButtonIconOnly + + + + + + 42 + 330 + 441 + 16 + + + + + 10 + + + + Ready... + + + + + + 40 + 350 + 520 + 23 + + + + QProgressBar { + border: 2px solid grey; + border-radius: 5px; + text-align: center; + } +QProgressBar::chunk { + background-color: #3D6699; + width: 10px; + margin: 0.5px; + } + + + 0 + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp new file mode 100644 index 000000000..46110a9e4 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.cpp @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * + * @file startpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Setup Wizard Plugin + * @{ + * @brief A Wizard to make the initial setup easy for everyone. + *****************************************************************************/ +/* + * 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 "startpage.h" +#include "ui_startpage.h" + +StartPage::StartPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::StartPage) +{ + ui->setupUi(this); +} + +StartPage::~StartPage() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h new file mode 100644 index 000000000..3e9e120cc --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.h @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * + * @file startpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Setup Wizard Plugin + * @{ + * @brief A Wizard to make the initial setup easy for everyone. + *****************************************************************************/ +/* + * 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 STARTPAGE_H +#define STARTPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class StartPage; +} + +class StartPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit StartPage(SetupWizard *wizard, QWidget *parent = 0); + ~StartPage(); + +private: + Ui::StartPage *ui; +}; + +#endif // STARTPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.ui new file mode 100644 index 000000000..3481e4b8e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/startpage.ui @@ -0,0 +1,59 @@ + + + StartPage + + + + 0 + 0 + 640 + 400 + + + + + 640 + 400 + + + + WizardPage + + + + + 20 + 20 + 581 + 350 + + + + <!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: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-size:12pt; font-weight:600;">Welcome to the OpenPilot Setup Wizard</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;"><br /></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-size:10pt;">This wizard will guide you through the basic steps required to setup your OpenPilot controller for the first time. You will be asked questions about your platform (multirotor/heli/fixed-wing) which this wizard will use to configure your aircraft for your maiden flight. </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-size:10pt;"><br /></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-size:10pt;">This wizard does not configure all of the advanced settings available in the GCS Configuration. All basic and advanced configuration parameters can be modified later by using the GCS Configuration plugin.</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-size:10pt;"><br /></p> +<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-size:12pt; font-weight:600; color:#ff0000;">WARNING: YOU MUST REMOVE ALL PROPELLERS </span></p> +<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-size:12pt; font-weight:600; color:#ff0000;">FROM THE VEHICLE BEFORE PROCEEDING!</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:'Lucida Grande'; font-size:13pt;"><br /></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-size:10pt;">Disregarding this warning puts you at</span><span style=" font-size:10pt; font-weight:600; color:#000000;"> risk of very serious injury</span><span style=" font-size:10pt;">!</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-size:10pt;"><br /></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-size:10pt;">Now that your props are removed we can get started. Ready?</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp new file mode 100644 index 000000000..c4846e782 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.cpp @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file summarypage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SummaryPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "summarypage.h" +#include "ui_summarypage.h" +#include "setupwizard.h" +#include "connectiondiagram.h" + +SummaryPage::SummaryPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::SummaryPage) +{ + ui->setupUi(this); + connect(ui->illustrationButton, SIGNAL(clicked()), this, SLOT(showDiagram())); +} + +SummaryPage::~SummaryPage() +{ + delete ui; +} + +bool SummaryPage::validatePage() +{ + return true; +} + +void SummaryPage::initializePage() +{ + ui->configurationSummary->setText(getWizard()->getSummaryText()); +} + +void SummaryPage::showDiagram() +{ + ConnectionDiagram diagram(this, getWizard()); + diagram.exec(); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h new file mode 100644 index 000000000..616f82392 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.h @@ -0,0 +1,54 @@ +/** + ****************************************************************************** + * + * @file summarypage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SummaryPage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 SUMMARYPAGE_H +#define SUMMARYPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class SummaryPage; +} + +class SummaryPage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit SummaryPage(SetupWizard *wizard, QWidget *parent = 0); + ~SummaryPage(); + bool validatePage(); + void initializePage(); + +private: + Ui::SummaryPage *ui; + +private slots: + void showDiagram(); +}; + +#endif // SUMMARYPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui new file mode 100644 index 000000000..879cfeb52 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui @@ -0,0 +1,117 @@ + + + SummaryPage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 541 + 241 + + + + <!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: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-size:12pt; font-weight:600;">OpenPilot configuration summary</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-size:12pt; font-weight:600;"><br /></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-size:10pt;">The first part of this wizard is now complete. All information required to create a basic OpenPilot controller configuration for a specific vehicle has been collected.</span></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-size:10pt;">Below is a summary of the configuration and a button that links to a diagram illustrating how to connect required hardware and the OpenPilot Controller with the current 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-size:10pt;"><br /></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-size:10pt;">To continue the wizard and go through some basic configuration steps, please continue to the next step of this wizard.</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-size:10pt;"><br /></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-size:10pt;">The following steps require that your OpenPilot controller is set up according to the diagram, it is </span><span style=" font-size:10pt; font-weight:600;">connected to the computer</span><span style=" font-size:10pt;"> by USB, and that the vehicle is</span><span style=" font-size:10pt; font-weight:600;"> powered by a battery</span><span style=" font-size:10pt;">.</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + 460 + 271 + 100 + 100 + + + + Show connection diagram for configuration + + + QToolButton { border: none } + + + + + + + :/setupwizard/resources/bttn-illustration-up.png + :/setupwizard/resources/bttn-illustration-down.png:/setupwizard/resources/bttn-illustration-up.png + + + + 100 + 100 + + + + false + + + false + + + true + + + Qt::ToolButtonIconOnly + + + + + + 20 + 270 + 400 + 100 + + + + + 10 + + + + Qt::ScrollBarAlwaysOn + + + Qt::ScrollBarAlwaysOff + + + Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.cpp new file mode 100644 index 000000000..a28724f2f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.cpp @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * + * @file surfacepage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SurfacePage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "surfacepage.h" +#include "ui_surfacepage.h" + +SurfacePage::SurfacePage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::SurfacePage) +{ + ui->setupUi(this); + setFinalPage(true); +} + +SurfacePage::~SurfacePage() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h new file mode 100644 index 000000000..593aa1fca --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file surfacepage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup SurfacePage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 SURFACEPAGE_H +#define SURFACEPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class SurfacePage; +} + +class SurfacePage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit SurfacePage(SetupWizard *wizard, QWidget *parent = 0); + ~SurfacePage(); + +private: + Ui::SurfacePage *ui; +}; + +#endif // SURFACEPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.ui new file mode 100644 index 000000000..d592a4eaf --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/surfacepage.ui @@ -0,0 +1,43 @@ + + + SurfacePage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 50 + 160 + 500 + 41 + + + + <!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-size:12pt; font-weight:600;">The Surface Vehicle section of the OpenPilot Setup Wizard is not yet implemented</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-size:8pt;"></p></body></html> + + + Qt::AlignHCenter|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp new file mode 100644 index 000000000..a616a1194 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file vehiclepage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehiclePage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "vehiclepage.h" +#include "ui_vehiclepage.h" + +VehiclePage::VehiclePage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::VehiclePage) +{ + ui->setupUi(this); +} + +VehiclePage::~VehiclePage() +{ + delete ui; +} + +bool VehiclePage::validatePage() +{ + if(ui->multirotorButton->isChecked()) { + getWizard()->setVehicleType(SetupWizard::VEHICLE_MULTI); + } + else if(ui->fixedwingButton->isChecked()) { + getWizard()->setVehicleType(SetupWizard::VEHICLE_FIXEDWING); + } + else if(ui->heliButton->isChecked()) { + getWizard()->setVehicleType(SetupWizard::VEHICLE_HELI); + } + else if(ui->surfaceButton->isChecked()) { + getWizard()->setVehicleType(SetupWizard::VEHICLE_SURFACE); + } + else { + getWizard()->setVehicleType(SetupWizard::VEHICLE_UNKNOWN); + } + return true; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h new file mode 100644 index 000000000..5e72a3d21 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h @@ -0,0 +1,50 @@ +/** + ****************************************************************************** + * + * @file vehiclepage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehiclePage + * @{ + * @brief + *****************************************************************************/ +/* + * 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 VEHICLEPAGE_H +#define VEHICLEPAGE_H + +#include "abstractwizardpage.h" + +namespace Ui { +class VehiclePage; +} + +class VehiclePage : public AbstractWizardPage +{ + Q_OBJECT + +public: + explicit VehiclePage(SetupWizard *wizard, QWidget *parent = 0); + ~VehiclePage(); + bool validatePage(); + +private: + Ui::VehiclePage *ui; +}; + +#endif // VEHICLEPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui new file mode 100644 index 000000000..804887b2d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui @@ -0,0 +1,256 @@ + + + VehiclePage + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 20 + 550 + 221 + + + + <!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: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-size:12pt; font-weight:600;">Vehicle type selection</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-size:12pt; font-weight:600;"><br /></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-size:10pt;">To continue, the wizard needs to know what type of vehicle the OpenPilot controller board is going to be used with. This step is crucial since much of the following configuration is unique per vehicle type.</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;"><br /></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-size:10pt;">Go ahead and select the type of vehicle for which you want to create a 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-size:10pt;"><br /></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-size:10pt;">(The current version only provides functionality for multirotors.)</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + false + + + + 430 + 250 + 100 + 120 + + + + + 10 + + + + Car, Boat, U-Boat + + + QToolButton { border: none } + + + Surface + + + + :/setupwizard/resources/bttn-land-up.png + :/setupwizard/resources/bttn-land-down.png:/setupwizard/resources/bttn-land-up.png + + + + 100 + 100 + + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + 70 + 250 + 100 + 120 + + + + + 10 + + + + Tricopter, Quadcopter, Hexacopter + + + false + + + QToolButton { border: none } + + + Multirotor + + + + :/setupwizard/resources/bttn-multi-up.png + :/setupwizard/resources/bttn-multi-down.png:/setupwizard/resources/bttn-multi-up.png + + + + 100 + 100 + + + + true + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + false + + + + 190 + 250 + 100 + 120 + + + + + 10 + + + + Airplane, Sloper, Jet + + + QToolButton { border: none } + + + Fixed wing + + + + :/setupwizard/resources/bttn-plane-up.png + :/setupwizard/resources/bttn-plane-down.png:/setupwizard/resources/bttn-plane-up.png + + + + 100 + 100 + + + + true + + + false + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + false + + + + 310 + 250 + 100 + 120 + + + + + 10 + + + + CCPM Helicopters + + + QToolButton { border: none } + + + Helicopter + + + + :/setupwizard/resources/bttn-heli-up.png + :/setupwizard/resources/bttn-heli-down.png:/setupwizard/resources/bttn-heli-up.png + + + + 100 + 100 + + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-down.png new file mode 100644 index 000000000..26899fb5a Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-up.png new file mode 100644 index 000000000..07016ddc1 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ESC-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-down.png new file mode 100644 index 000000000..5681d474e Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-up.png new file mode 100644 index 000000000..c2e2add72 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-calculate-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-down.png new file mode 100644 index 000000000..0d75a7ba8 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-over.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-over.png new file mode 100644 index 000000000..d979da128 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-over.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-up.png new file mode 100644 index 000000000..49236a2b6 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-heli-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-down.png new file mode 100644 index 000000000..a4402d5b0 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-up.png new file mode 100644 index 000000000..5013d30b2 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-illustration-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-down.png new file mode 100644 index 000000000..ed2c66934 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-over.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-over.png new file mode 100644 index 000000000..8d6336bc4 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-over.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-up.png new file mode 100644 index 000000000..7d90f4a2d Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-land-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-down.png new file mode 100644 index 000000000..7da5b4fdc Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-over.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-over.png new file mode 100644 index 000000000..82a017767 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-over.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-up.png new file mode 100644 index 000000000..882d6edfa Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-multi-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-down.png new file mode 100644 index 000000000..42bfddd28 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-over.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-over.png new file mode 100644 index 000000000..d99efed99 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-over.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-up.png new file mode 100644 index 000000000..4ccb6567d Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-plane-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-down.png new file mode 100644 index 000000000..03706d360 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-up.png new file mode 100644 index 000000000..f58e5596b Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-ppm-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-down.png new file mode 100644 index 000000000..72656896a Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-up.png new file mode 100644 index 000000000..25d3624dd Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-pwm-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-down.png new file mode 100644 index 000000000..05bc35895 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-up.png new file mode 100644 index 000000000..37e16c2cb Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sat-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-down.png new file mode 100644 index 000000000..13b6e3fa5 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-up.png new file mode 100644 index 000000000..9e6655d0c Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-save-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-down.png new file mode 100644 index 000000000..ffea88361 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-up.png new file mode 100644 index 000000000..9937ac0b4 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-sbus-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-down.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-down.png new file mode 100644 index 000000000..2a6f0a8db Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-down.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-up.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-up.png new file mode 100644 index 000000000..7a8013432 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-turbo-up.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-off.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-off.png new file mode 100644 index 000000000..bb0aab304 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-off.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-on.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-on.png new file mode 100644 index 000000000..98eecd4ef Binary files /dev/null and b/ground/openpilotgcs/src/plugins/setupwizard/resources/bttn-txwizard-on.png differ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg new file mode 100644 index 000000000..1f2219f3f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/resources/connection-diagrams.svg @@ -0,0 +1,8011 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Spectrum + Satellite + + + + + + + + + + + + Futaba + S-Bus + + + + + + + + + + + + PPM Signal + + + + + + + + + + + + + + + + + Throttle + Roll + Pitch + Yaw + Flight Mode + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/multirotor-shapes.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/multirotor-shapes.svg new file mode 100644 index 000000000..a3077cbfd --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/resources/multirotor-shapes.svg @@ -0,0 +1,7219 @@ + + + +image/svg+xml \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp new file mode 100644 index 000000000..fd3b2a083 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -0,0 +1,306 @@ +/** + ****************************************************************************** + * + * @file setupwizard.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Setup Wizard Plugin + * @{ + * @brief A Wizard to make the initial setup easy for everyone. + *****************************************************************************/ +/* + * 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 "setupwizard.h" +#include "pages/startpage.h" +#include "pages/endpage.h" +#include "pages/controllerpage.h" +#include "pages/vehiclepage.h" +#include "pages/multipage.h" +#include "pages/fixedwingpage.h" +#include "pages/helipage.h" +#include "pages/surfacepage.h" +#include "pages/inputpage.h" +#include "pages/outputpage.h" +#include "pages/levellingpage.h" +#include "pages/summarypage.h" +#include "pages/savepage.h" +#include "pages/notyetimplementedpage.h" +#include "pages/rebootpage.h" +#include "pages/outputcalibrationpage.h" +#include "extensionsystem/pluginmanager.h" +#include "vehicleconfigurationhelper.h" +#include "actuatorsettings.h" + +SetupWizard::SetupWizard(QWidget *parent) : QWizard(parent), VehicleConfigurationSource(), + m_controllerType(CONTROLLER_UNKNOWN), + m_vehicleType(VEHICLE_UNKNOWN), m_inputType(INPUT_UNKNOWN), m_escType(ESC_UNKNOWN), + m_levellingPerformed(false), m_restartNeeded(false), m_connectionManager(0) +{ + setWindowTitle(tr("OpenPilot Setup Wizard")); + setOption(QWizard::IndependentPages, false); + for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) + { + m_actuatorSettings << actuatorChannelSettings(); + } + createPages(); + setWizardStyle(QWizard::ModernStyle); + setFixedSize(640, 530); +} + +int SetupWizard::nextId() const +{ + switch (currentId()) { + case PAGE_START: + return PAGE_CONTROLLER; + case PAGE_CONTROLLER: { + switch(getControllerType()) + { + case CONTROLLER_CC: + case CONTROLLER_CC3D: + return PAGE_INPUT; + case CONTROLLER_REVO: + case CONTROLLER_PIPX: + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_VEHICLES: { + switch(getVehicleType()) + { + case VEHICLE_MULTI: + return PAGE_MULTI; + case VEHICLE_FIXEDWING: + return PAGE_FIXEDWING; + case VEHICLE_HELI: + return PAGE_HELI; + case VEHICLE_SURFACE: + return PAGE_SURFACE; + default: + return PAGE_NOTYETIMPLEMENTED; + } + } + case PAGE_MULTI: + return PAGE_OUTPUT; + case PAGE_INPUT: + if(isRestartNeeded()) { + saveHardwareSettings(); + return PAGE_REBOOT; + } + else { + return PAGE_VEHICLES; + } + case PAGE_REBOOT: + return PAGE_VEHICLES; + case PAGE_OUTPUT: + return PAGE_SUMMARY; + case PAGE_LEVELLING: + return PAGE_CALIBRATION; + case PAGE_CALIBRATION: + return PAGE_SAVE; + case PAGE_SUMMARY: + return PAGE_LEVELLING; + case PAGE_SAVE: + return PAGE_END; + case PAGE_NOTYETIMPLEMENTED: + return PAGE_END; + default: + return -1; + } +} + +QString SetupWizard::getSummaryText() +{ + QString summary = ""; + summary.append("").append(tr("Controller type: ")).append(""); + switch(getControllerType()) + { + case CONTROLLER_CC: + summary.append(tr("OpenPilot CopterControl")); + break; + case CONTROLLER_CC3D: + summary.append(tr("OpenPilot CopterControl 3D")); + break; + case CONTROLLER_REVO: + summary.append(tr("OpenPilot Revolution")); + break; + case CONTROLLER_PIPX: + summary.append(tr("OpenPilot PipX Radio Modem")); + break; + default: + summary.append(tr("Unknown")); + break; + } + + summary.append("
"); + summary.append("").append(tr("Vehicle type: ")).append(""); + switch (getVehicleType()) + { + case VEHICLE_MULTI: + summary.append(tr("Multirotor")); + + summary.append("
"); + summary.append("").append(tr("Vehicle sub type: ")).append(""); + switch (getVehicleSubType()) + { + case SetupWizard::MULTI_ROTOR_TRI_Y: + summary.append(tr("Tricopter")); + break; + case SetupWizard::MULTI_ROTOR_QUAD_X: + summary.append(tr("Quadcopter X")); + break; + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + summary.append(tr("Quadcopter +")); + break; + case SetupWizard::MULTI_ROTOR_HEXA: + summary.append(tr("Hexacopter")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + summary.append(tr("Hexacopter Coax (Y6)")); + break; + case SetupWizard::MULTI_ROTOR_HEXA_H: + summary.append(tr("Hexacopter X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO: + summary.append(tr("Octocopter")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_X: + summary.append(tr("Octocopter Coax X")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_COAX_PLUS: + summary.append(tr("Octocopter Coax +")); + break; + case SetupWizard::MULTI_ROTOR_OCTO_V: + summary.append(tr("Octocopter V")); + break; + default: + summary.append(tr("Unknown")); + break; + } + + break; + case VEHICLE_FIXEDWING: + summary.append(tr("Fixed wing")); + break; + case VEHICLE_HELI: + summary.append(tr("Helicopter")); + break; + case VEHICLE_SURFACE: + summary.append(tr("Surface vehicle")); + break; + default: + summary.append(tr("Unknown")); + } + + summary.append("
"); + summary.append("").append(tr("Input type: ")).append(""); + switch (getInputType()) + { + case INPUT_PWM: + summary.append(tr("PWM (One cable per channel)")); + break; + case INPUT_PPM: + summary.append(tr("PPM (One cable for all channels)")); + break; + case INPUT_SBUS: + summary.append(tr("Futaba S.Bus")); + break; + case INPUT_DSM2: + summary.append(tr("Spectrum satellite (DSM2)")); + break; + case INPUT_DSMX10: + summary.append(tr("Spectrum satellite (DSMX10BIT)")); + break; + case INPUT_DSMX11: + summary.append(tr("Spectrum satellite (DSMX11BIT)")); + break; + default: + summary.append(tr("Unknown")); + } + + summary.append("
"); + summary.append("").append(tr("ESC type: ")).append(""); + switch (getESCType()) + { + case ESC_LEGACY: + summary.append(tr("Legacy ESC (50 Hz)")); + break; + case ESC_RAPID: + summary.append(tr("Rapid ESC (400 Hz)")); + break; + default: + summary.append(tr("Unknown")); + } + + /* + summary.append("
"); + summary.append("").append(tr("Reboot required: ")).append(""); + summary.append(isRestartNeeded() ? tr("Yes") : tr("No")); + */ + return summary; +} + +void SetupWizard::createPages() +{ + setPage(PAGE_START, new StartPage(this)); + setPage(PAGE_CONTROLLER, new ControllerPage(this)); + setPage(PAGE_VEHICLES, new VehiclePage(this)); + setPage(PAGE_MULTI, new MultiPage(this)); + setPage(PAGE_FIXEDWING, new FixedWingPage(this)); + setPage(PAGE_HELI, new HeliPage(this)); + setPage(PAGE_SURFACE, new SurfacePage(this)); + setPage(PAGE_INPUT, new InputPage(this)); + setPage(PAGE_OUTPUT, new OutputPage(this)); + setPage(PAGE_LEVELLING, new LevellingPage(this)); + setPage(PAGE_CALIBRATION, new OutputCalibrationPage(this)); + setPage(PAGE_SUMMARY, new SummaryPage(this)); + setPage(PAGE_SAVE, new SavePage(this)); + setPage(PAGE_REBOOT, new RebootPage(this)); + setPage(PAGE_NOTYETIMPLEMENTED, new NotYetImplementedPage(this)); + setPage(PAGE_END, new EndPage(this)); + + setStartId(PAGE_START); + + connect(button(QWizard::CustomButton1), SIGNAL(clicked()), this, SLOT(customBackClicked())); + setButtonText(QWizard::CustomButton1, buttonText(QWizard::BackButton)); + QList button_layout; + button_layout << QWizard::Stretch << QWizard::CustomButton1 << QWizard::NextButton << QWizard::CancelButton << QWizard::FinishButton; + setButtonLayout(button_layout); + connect(this, SIGNAL(currentIdChanged(int)), this, SLOT(pageChanged(int))); +} + +void SetupWizard::customBackClicked() +{ + if(currentId() == PAGE_CALIBRATION) { + static_cast(currentPage())->customBackClicked(); + } + else { + back(); + } +} + +void SetupWizard::pageChanged(int currId) +{ + button(QWizard::CustomButton1)->setVisible(currId != PAGE_START); + button(QWizard::CancelButton)->setVisible(currId != PAGE_END); +} + +bool SetupWizard::saveHardwareSettings() const +{ + VehicleConfigurationHelper helper(const_cast(this)); + return helper.setupHardwareSettings(); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h new file mode 100644 index 000000000..c6cf7e1ae --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * + * @file setupwizard.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup Setup Wizard Plugin + * @{ + * @brief A Wizards to make the initial setup easy for everyone. + *****************************************************************************/ +/* + * 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 SETUPWIZARD_H +#define SETUPWIZARD_H + +#include +#include "levellingutil.h" +#include +#include +#include "vehicleconfigurationsource.h" +#include "vehicleconfigurationhelper.h" + +class SetupWizard : public QWizard, public VehicleConfigurationSource +{ + Q_OBJECT + +public: + SetupWizard(QWidget *parent = 0); + int nextId() const; + + void setControllerType(SetupWizard::CONTROLLER_TYPE type) { m_controllerType = type; } + SetupWizard::CONTROLLER_TYPE getControllerType() const { return m_controllerType; } + + void setVehicleType(SetupWizard::VEHICLE_TYPE type) { m_vehicleType = type; } + SetupWizard::VEHICLE_TYPE getVehicleType() const { return m_vehicleType; } + + void setVehicleSubType(SetupWizard::VEHICLE_SUB_TYPE type) { m_vehicleSubType = type; } + SetupWizard::VEHICLE_SUB_TYPE getVehicleSubType() const { return m_vehicleSubType; } + + void setInputType(SetupWizard::INPUT_TYPE type) { m_inputType = type; } + SetupWizard::INPUT_TYPE getInputType() const { return m_inputType; } + + void setESCType(SetupWizard::ESC_TYPE type) { m_escType = type; } + SetupWizard::ESC_TYPE getESCType() const { return m_escType; } + + void setLevellingBias(accelGyroBias bias) { m_levellingBias = bias; m_levellingPerformed = true; } + bool isLevellingPerformed() const { return m_levellingPerformed; } + accelGyroBias getLevellingBias() const { return m_levellingBias; } + + void setActuatorSettings(QList actuatorSettings) { m_actuatorSettings = actuatorSettings; } + bool isMotorCalibrationPerformed() const { return m_motorCalibrationPerformed; } + QList getActuatorSettings() const { return m_actuatorSettings; } + + void setRestartNeeded(bool needed) { m_restartNeeded = needed; } + bool isRestartNeeded() const {return m_restartNeeded; } + + QString getSummaryText(); + + Core::ConnectionManager* getConnectionManager() { + if (!m_connectionManager) { + m_connectionManager = Core::ICore::instance()->connectionManager(); + Q_ASSERT(m_connectionManager); + } + return m_connectionManager; + } + +private slots: + void customBackClicked(); + void pageChanged(int currId); +private: + enum {PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, + PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_LEVELLING, PAGE_CALIBRATION, + PAGE_SAVE, PAGE_SUMMARY, PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END}; + void createPages(); + bool saveHardwareSettings() const; + + CONTROLLER_TYPE m_controllerType; + VEHICLE_TYPE m_vehicleType; + VEHICLE_SUB_TYPE m_vehicleSubType; + INPUT_TYPE m_inputType; + ESC_TYPE m_escType; + + bool m_levellingPerformed; + accelGyroBias m_levellingBias; + + bool m_motorCalibrationPerformed; + QList m_actuatorSettings; + + bool m_restartNeeded; + + bool m_back; + + Core::ConnectionManager *m_connectionManager; +}; + +#endif // SETUPWIZARD_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro new file mode 100644 index 000000000..6d336a888 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -0,0 +1,83 @@ + +TEMPLATE = lib +TARGET = SetupWizard +QT += svg + +include(../../openpilotgcsplugin.pri) +include(../../plugins/coreplugin/coreplugin.pri) +include(../../plugins/uavobjectutil/uavobjectutil.pri) +include(../../plugins/config/config.pri) + +HEADERS += setupwizardplugin.h \ + setupwizard.h \ + pages/startpage.h \ + pages/endpage.h \ + pages/controllerpage.h \ + pages/vehiclepage.h \ + pages/notyetimplementedpage.h \ + pages/multipage.h \ + pages/fixedwingpage.h \ + pages/helipage.h \ + pages/surfacepage.h \ + pages/abstractwizardpage.h \ + pages/outputpage.h \ + pages/inputpage.h \ + pages/summarypage.h \ + pages/levellingpage.h \ + levellingutil.h \ + vehicleconfigurationsource.h \ + vehicleconfigurationhelper.h \ + connectiondiagram.h \ + pages/outputcalibrationpage.h \ + outputcalibrationutil.h \ + pages/rebootpage.h \ + pages/savepage.h + +SOURCES += setupwizardplugin.cpp \ + setupwizard.cpp \ + pages/startpage.cpp \ + pages/endpage.cpp \ + pages/controllerpage.cpp \ + pages/vehiclepage.cpp \ + pages/notyetimplementedpage.cpp \ + pages/multipage.cpp \ + pages/fixedwingpage.cpp \ + pages/helipage.cpp \ + pages/surfacepage.cpp \ + pages/abstractwizardpage.cpp \ + pages/outputpage.cpp \ + pages/inputpage.cpp \ + pages/summarypage.cpp \ + pages/levellingpage.cpp \ + levellingutil.cpp \ + vehicleconfigurationsource.cpp \ + vehicleconfigurationhelper.cpp \ + connectiondiagram.cpp \ + pages/outputcalibrationpage.cpp \ + outputcalibrationutil.cpp \ + pages/rebootpage.cpp \ + pages/savepage.cpp + +OTHER_FILES += SetupWizard.pluginspec + +FORMS += \ + pages/startpage.ui \ + pages/endpage.ui \ + pages/controllerpage.ui \ + pages/vehiclepage.ui \ + pages/notyetimplementedpage.ui \ + pages/multipage.ui \ + pages/fixedwingpage.ui \ + pages/helipage.ui \ + pages/surfacepage.ui \ + pages/outputpage.ui \ + pages/inputpage.ui \ + pages/summarypage.ui \ + pages/levellingpage.ui \ + connectiondiagram.ui \ + pages/outputcalibrationpage.ui \ + pages/rebootpage.ui \ + pages/savepage.ui + +RESOURCES += \ + wizardResources.qrc diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp new file mode 100644 index 000000000..d36a747be --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.cpp @@ -0,0 +1,88 @@ +/** + ****************************************************************************** + * + * @file setupwizardplugin.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup SetupWizardPlugin + * @{ + * @brief A Setup Wizard 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 "setupwizardplugin.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +SetupWizardPlugin::SetupWizardPlugin() +{ + // Do nothing +} + +SetupWizardPlugin::~SetupWizardPlugin() +{ +} + +bool SetupWizardPlugin::initialize(const QStringList& args, QString *errMsg) +{ + Q_UNUSED(args); + Q_UNUSED(errMsg); + + // Add Menu entry + Core::ActionManager* am = Core::ICore::instance()->actionManager(); + Core::ActionContainer* ac = am->actionContainer(Core::Constants::M_TOOLS); + + Core::Command* cmd = am->registerAction(new QAction(this), + "SetupWizardPlugin.ShowSetupWizard", + QList() << + Core::Constants::C_GLOBAL_ID); + cmd->setDefaultKeySequence(QKeySequence("Ctrl+V")); + cmd->action()->setText(tr("Vehicle Setup Wizard")); + + Core::ModeManager::instance()->addAction(cmd, 1); + + ac->menu()->addSeparator(); + ac->appendGroup("Wizard"); + ac->addAction(cmd, "Wizard"); + + connect(cmd->action(), SIGNAL(triggered(bool)), this, SLOT(showSetupWizard())); + return true; +} + +void SetupWizardPlugin::extensionsInitialized() +{ +} + +void SetupWizardPlugin::shutdown() +{ +} + +void SetupWizardPlugin::showSetupWizard() +{ + SetupWizard().exec(); +} + +Q_EXPORT_PLUGIN(SetupWizardPlugin) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h new file mode 100644 index 000000000..96ca9e038 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizardplugin.h @@ -0,0 +1,50 @@ +/** + ****************************************************************************** + * + * @file setupwizardplugin.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup SetupWizardPlugin + * @{ + * @brief A Setup Wizard 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 SETUPWIZARDPLUGIN_H +#define SETUPWIZARDPLUGIN_H + +#include +#include +#include "setupwizard.h" + +class SetupWizardPlugin : public ExtensionSystem::IPlugin +{ + Q_OBJECT +public: + SetupWizardPlugin(); + ~SetupWizardPlugin(); + + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString * errorString); + void shutdown(); + +private slots: + void showSetupWizard(); + +}; + +#endif // SETUPWIZARDPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp new file mode 100644 index 000000000..709810bce --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -0,0 +1,1202 @@ +/** + ****************************************************************************** + * + * @file vehicleconfigurationhelper.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehicleConfigurationHelper + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "vehicleconfigurationhelper.h" +#include "extensionsystem/pluginmanager.h" +#include "hwsettings.h" +#include "actuatorsettings.h" +#include "attitudesettings.h" +#include "mixersettings.h" +#include "systemsettings.h" +#include "manualcontrolsettings.h" +#include "stabilizationsettings.h" + +const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; +const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; + +VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) + : m_configSource(configSource), m_uavoManager(0), + m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), + m_progress(0) +{ + Q_ASSERT(m_configSource); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + m_uavoManager = pm->getObject(); + Q_ASSERT(m_uavoManager); +} + +bool VehicleConfigurationHelper::setupVehicle(bool save) +{ + m_progress = 0; + clearModifiedObjects(); + resetVehicleConfig(); + resetGUIData(); + if(!saveChangesToController(save)) { + return false; + } + + m_progress = 0; + applyHardwareConfiguration(); + applyVehicleConfiguration(); + applyActuatorConfiguration(); + applyFlighModeConfiguration(); + applyLevellingConfiguration(); + applyStabilizationConfiguration(); + applyManualControlDefaults(); + + bool result = saveChangesToController(save); + emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, result ? tr("Done!") : tr("Failed!")); + return result; +} + +bool VehicleConfigurationHelper::setupHardwareSettings(bool save) +{ + m_progress = 0; + clearModifiedObjects(); + applyHardwareConfiguration(); + applyManualControlDefaults(); + + bool result = saveChangesToController(save); + emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, result ? tr("Done!") : tr("Failed!")); + return result; +} + +void VehicleConfigurationHelper::addModifiedObject(UAVDataObject *object, QString description) +{ + m_modifiedObjects << new QPair(object, description); +} + +void VehicleConfigurationHelper::clearModifiedObjects() +{ + for(int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *pair = m_modifiedObjects.at(i); + delete pair; + } + m_modifiedObjects.clear(); +} + +void VehicleConfigurationHelper::applyHardwareConfiguration() +{ + HwSettings* hwSettings = HwSettings::GetInstance(m_uavoManager); + HwSettings::DataFields data = hwSettings->getData(); + switch(m_configSource->getControllerType()) + { + case VehicleConfigurationSource::CONTROLLER_CC: + case VehicleConfigurationSource::CONTROLLER_CC3D: + // Reset all ports + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_DISABLED; + + //Default flexiport to be active telemetry link + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_TELEMETRY; + + data.CC_MainPort = HwSettings::CC_MAINPORT_DISABLED; + switch(m_configSource->getInputType()) + { + case VehicleConfigurationSource::INPUT_PWM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + data.CC_RcvrPort = HwSettings::CC_RCVRPORT_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + data.CC_MainPort = HwSettings::CC_MAINPORT_SBUS; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + data.CC_MainPort = HwSettings::CC_MAINPORT_DSMX10BIT; + break; + case VehicleConfigurationSource::INPUT_DSMX11: + data.CC_MainPort = HwSettings::CC_MAINPORT_DSMX11BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2: + data.CC_MainPort = HwSettings::CC_MAINPORT_DSM2; + break; + default: + break; + } + break; + case VehicleConfigurationSource::CONTROLLER_REVO: + // TODO: Implement Revo settings + break; + default: + break; + } + hwSettings->setData(data); + addModifiedObject(hwSettings, tr("Writing hardware settings")); +} + +void VehicleConfigurationHelper::applyVehicleConfiguration() +{ + + switch(m_configSource->getVehicleType()) + { + case VehicleConfigurationSource::VEHICLE_MULTI: + { + switch(m_configSource->getVehicleSubType()) + { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + setupTriCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + setupQuadCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + setupHexaCopter(); + break; + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + setupOctoCopter(); + break; + default: + break; + } + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; + } +} + +void VehicleConfigurationHelper::applyActuatorConfiguration() +{ + ActuatorSettings* actSettings = ActuatorSettings::GetInstance(m_uavoManager); + switch(m_configSource->getVehicleType()) { + case VehicleConfigurationSource::VEHICLE_MULTI: { + ActuatorSettings::DataFields data = actSettings->getData(); + + QList actuatorSettings = m_configSource->getActuatorSettings(); + for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { + data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; + data.ChannelAddr[i] = i; + data.ChannelMin[i] = actuatorSettings[i].channelMin; + data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; + data.ChannelMax[i] = actuatorSettings[i].channelMax; + } + + data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; + + for(quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; + } + + qint16 updateFrequence = LEGACY_ESC_FREQUENCE; + switch(m_configSource->getESCType()) { + case VehicleConfigurationSource::ESC_LEGACY: + updateFrequence = LEGACY_ESC_FREQUENCE; + break; + case VehicleConfigurationSource::ESC_RAPID: + updateFrequence = RAPID_ESC_FREQUENCE; + break; + default: + break; + } + + switch(m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y: + data.ChannelUpdateFreq[0] = updateFrequence; + break; + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + break; + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: + data.ChannelUpdateFreq[0] = updateFrequence; + data.ChannelUpdateFreq[1] = updateFrequence; + data.ChannelUpdateFreq[2] = updateFrequence; + data.ChannelUpdateFreq[3] = updateFrequence; + break; + default: + break; + } + actSettings->setData(data); + addModifiedObject(actSettings, tr("Writing actuator settings")); + break; + } + case VehicleConfigurationSource::VEHICLE_FIXEDWING: + case VehicleConfigurationSource::VEHICLE_HELI: + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for other vehicle types? + break; + default: + break; + } +} + +void VehicleConfigurationHelper::applyFlighModeConfiguration() +{ + ManualControlSettings* controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(controlSettings); + + ManualControlSettings::DataFields data = controlSettings->getData(); + data.Stabilization1Settings[0] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE; + data.Stabilization1Settings[1] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE; + data.Stabilization1Settings[2] = ManualControlSettings::STABILIZATION1SETTINGS_AXISLOCK; + data.Stabilization2Settings[0] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE; + data.Stabilization2Settings[1] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE; + data.Stabilization2Settings[2] = ManualControlSettings::STABILIZATION2SETTINGS_RATE; + data.Stabilization3Settings[0] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; + data.Stabilization3Settings[1] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; + data.Stabilization3Settings[2] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; + data.FlightModeNumber = 3; + data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; + controlSettings->setData(data); + addModifiedObject(controlSettings, tr("Writing flight mode settings")); +} + +void VehicleConfigurationHelper::applyLevellingConfiguration() +{ + if(m_configSource->isLevellingPerformed()) + { + accelGyroBias bias = m_configSource->getLevellingBias(); + AttitudeSettings* attitudeSettings = AttitudeSettings::GetInstance(m_uavoManager); + Q_ASSERT(attitudeSettings); + AttitudeSettings::DataFields data = attitudeSettings->getData(); + + data.AccelBias[0] += bias.m_accelerometerXBias; + data.AccelBias[1] += bias.m_accelerometerYBias; + data.AccelBias[2] += bias.m_accelerometerZBias; + data.GyroBias[0] = -bias.m_gyroXBias; + data.GyroBias[1] = -bias.m_gyroYBias; + data.GyroBias[2] = -bias.m_gyroZBias; + + attitudeSettings->setData(data); + addModifiedObject(attitudeSettings, tr("Writing gyro and accelerometer bias settings")); + } +} + +void VehicleConfigurationHelper::applyStabilizationConfiguration() +{ + StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); + Q_ASSERT(stabSettings); + StabilizationSettings::DataFields data = stabSettings->getData(); + + StabilizationSettings defaultSettings; + stabSettings->setData(defaultSettings.getData()); + addModifiedObject(stabSettings, tr("Writing stabilization settings")); +} + +void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings channels[]) +{ + // Set all mixer data + MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + Q_ASSERT(mSettings); + + // Set Mixer types and values + QString mixerTypePattern = "Mixer%1Type"; + QString mixerVectorPattern = "Mixer%1Vector"; + for(int i = 0; i < 10; i++) { + UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i + 1)); + Q_ASSERT(field); + field->setValue(field->getOptions().at(channels[i].type)); + + field = mSettings->getField(mixerVectorPattern.arg(i + 1)); + Q_ASSERT(field); + field->setValue((channels[i].throttle1 * 127) / 100, 0); + field->setValue((channels[i].throttle2 * 127) / 100, 1); + field->setValue((channels[i].roll * 127) / 100, 2); + field->setValue((channels[i].pitch * 127) / 100, 3); + field->setValue((channels[i].yaw *127) / 100, 4); + } + + // Apply updates + mSettings->setData(mSettings->getData()); + addModifiedObject(mSettings, tr("Writing mixer settings")); + +} + +void VehicleConfigurationHelper::applyMultiGUISettings(SystemSettings::AirframeTypeOptions airframe, GUIConfigDataUnion guiConfig) +{ + SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); + SystemSettings::DataFields data = sSettings->getData(); + data.AirframeType = airframe; + + for (int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { + data.GUIConfigData[i] = guiConfig.UAVObject[i]; + } + + sSettings->setData(data); + addModifiedObject(sSettings, tr("Writing vehicle settings")); +} + +void VehicleConfigurationHelper::applyManualControlDefaults() +{ + ManualControlSettings *mcSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(mcSettings); + ManualControlSettings::DataFields cData = mcSettings->getData(); + + ManualControlSettings::ChannelGroupsOptions channelType = ManualControlSettings::CHANNELGROUPS_PWM; + switch(m_configSource->getInputType()) + { + case VehicleConfigurationSource::INPUT_PWM: + channelType = ManualControlSettings::CHANNELGROUPS_PWM; + break; + case VehicleConfigurationSource::INPUT_PPM: + channelType = ManualControlSettings::CHANNELGROUPS_PPM; + break; + case VehicleConfigurationSource::INPUT_SBUS: + channelType = ManualControlSettings::CHANNELGROUPS_SBUS; + break; + case VehicleConfigurationSource::INPUT_DSMX10: + case VehicleConfigurationSource::INPUT_DSMX11: + case VehicleConfigurationSource::INPUT_DSM2: + channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; + break; + default: + break; + } + + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_THROTTLE] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_ROLL] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_YAW] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_PITCH] = channelType; + cData.ChannelGroups[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = channelType; + + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_THROTTLE] = 1; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_ROLL] = 2; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_YAW] = 3; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_PITCH] = 4; + cData.ChannelNumber[ManualControlSettings::CHANNELGROUPS_FLIGHTMODE] = 5; + + mcSettings->setData(cData); + addModifiedObject(mcSettings, tr("Writing manual control defaults")); +} + +bool VehicleConfigurationHelper::saveChangesToController(bool save) +{ + qDebug() << "Saving modified objects to controller. " << m_modifiedObjects.count() << " objects in found."; + const int OUTER_TIMEOUT = 3000 * 20; // 10 seconds timeout for saving all objects + const int INNER_TIMEOUT = 2000; // 1 second timeout on every save attempt + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); + UAVObjectUtilManager* utilMngr = pm->getObject(); + Q_ASSERT(utilMngr); + + QTimer outerTimeoutTimer; + outerTimeoutTimer.setSingleShot(true); + + QTimer innerTimeoutTimer; + innerTimeoutTimer.setSingleShot(true); + + connect(utilMngr, SIGNAL(saveCompleted(int ,bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); + connect(&innerTimeoutTimer, SIGNAL(timeout()), &m_eventLoop, SLOT(quit())); + connect(&outerTimeoutTimer, SIGNAL(timeout()), this, SLOT(saveChangesTimeout())); + + outerTimeoutTimer.start(OUTER_TIMEOUT); + for(int i = 0; i < m_modifiedObjects.count(); i++) { + QPair *objPair = m_modifiedObjects.at(i); + m_transactionOK = false; + UAVDataObject* obj = objPair->first; + QString objDescription = objPair->second; + if(UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) { + + emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, objDescription); + + m_currentTransactionObjectID = obj->getObjID(); + + connect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); + while(!m_transactionOK && !m_transactionTimeout) { + // Allow the transaction to take some time + innerTimeoutTimer.start(INNER_TIMEOUT); + + // Set object updated + obj->updated(); + if(!m_transactionOK) { + m_eventLoop.exec(); + } + innerTimeoutTimer.stop(); + } + disconnect(obj, SIGNAL(transactionCompleted(UAVObject* ,bool)), this, SLOT(uAVOTransactionCompleted(UAVObject*, bool))); + if(m_transactionOK) { + qDebug() << "Object " << obj->getName() << " was successfully updated."; + if(save) { + m_transactionOK = false; + m_currentTransactionObjectID = obj->getObjID(); + // Try to save until success or timeout + while(!m_transactionOK && !m_transactionTimeout) { + // Allow the transaction to take some time + innerTimeoutTimer.start(INNER_TIMEOUT); + + // Persist object in controller + utilMngr->saveObjectToSD(obj); + if(!m_transactionOK) { + m_eventLoop.exec(); + } + innerTimeoutTimer.stop(); + } + m_currentTransactionObjectID = -1; + } + } + + if(!m_transactionOK) { + qDebug() << "Transaction timed out when trying to save: " << obj->getName(); + } + else { + qDebug() << "Object " << obj->getName() << " was successfully saved."; + } + } + else { + qDebug() << "Trying to save a UAVDataObject that is read only or is not a settings object."; + } + if(m_transactionTimeout) { + qDebug() << "Transaction timed out when trying to save " << m_modifiedObjects.count() << " objects."; + break; + } + } + + outerTimeoutTimer.stop(); + disconnect(&outerTimeoutTimer, SIGNAL(timeout()), this, SLOT(saveChangesTimeout())); + disconnect(&innerTimeoutTimer, SIGNAL(timeout()), &m_eventLoop, SLOT(quit())); + disconnect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(uAVOTransactionCompleted(int, bool))); + + qDebug() << "Finished saving modified objects to controller. Success = " << m_transactionOK; + + return m_transactionOK; +} + +void VehicleConfigurationHelper::uAVOTransactionCompleted(int oid, bool success) +{ + if(oid == m_currentTransactionObjectID) + { + m_transactionOK = success; + m_eventLoop.quit(); + } +} + +void VehicleConfigurationHelper::uAVOTransactionCompleted(UAVObject *object, bool success) +{ + if(object) { + uAVOTransactionCompleted(object->getObjID(), success); + } +} + +void VehicleConfigurationHelper::saveChangesTimeout() +{ + m_transactionOK = false; + m_transactionTimeout = true; + m_eventLoop.quit(); +} + +void VehicleConfigurationHelper::resetVehicleConfig() +{ + // Reset all vehicle data + MixerSettings* mSettings = MixerSettings::GetInstance(m_uavoManager); + + // Reset feed forward, accel times etc + mSettings->setFeedForward(0.0f); + mSettings->setMaxAccel(1000.0f); + mSettings->setAccelTime(0.0f); + mSettings->setDecelTime(0.0f); + + // Reset throttle curves + QString throttlePattern = "ThrottleCurve%1"; + for(int i = 1; i <= 2; i++) { + UAVObjectField *field = mSettings->getField(throttlePattern.arg(i)); + Q_ASSERT(field); + for(quint32 i = 0; i < field->getNumElements(); i++){ + field->setValue(i * ( 1.0f / (field->getNumElements() - 1)), i); + } + } + + // Reset Mixer types and values + QString mixerTypePattern = "Mixer%1Type"; + QString mixerVectorPattern = "Mixer%1Vector"; + for(int i = 1; i <= 10; i++) { + UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i)); + Q_ASSERT(field); + field->setValue(field->getOptions().at(0)); + + field = mSettings->getField(mixerVectorPattern.arg(i)); + Q_ASSERT(field); + for(quint32 i = 0; i < field->getNumElements(); i++){ + field->setValue(0, i); + } + } + + // Apply updates + //mSettings->setData(mSettings->getData()); + addModifiedObject(mSettings, tr("Preparing mixer settings")); +} + +void VehicleConfigurationHelper::resetGUIData() +{ + SystemSettings * sSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(sSettings); + SystemSettings::DataFields data = sSettings->getData(); + data.AirframeType = SystemSettings::AIRFRAMETYPE_CUSTOM; + for(quint32 i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) { + data.GUIConfigData[i] = 0; + } + sSettings->setData(data); + addModifiedObject(sSettings, tr("Preparing vehicle settings")); +} + + +void VehicleConfigurationHelper::setupTriCopter() +{ + // Typical vehicle setup + // 1. Setup mixer data + // 2. Setup GUI data + // 3. Apply changes + + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 50; + channels[0].yaw = 0; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 50; + channels[1].yaw = 0; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = 0; + + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 100; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.TRIYaw = 4; + + applyMixerConfiguration(channels); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_TRI, guiSettings); +} + +GUIConfigDataUnion VehicleConfigurationHelper::getGUIConfigData() +{ + GUIConfigDataUnion configData; + + SystemSettings * systemSettings = SystemSettings::GetInstance(m_uavoManager); + Q_ASSERT(systemSettings); + SystemSettings::DataFields systemSettingsData = systemSettings->getData(); + + for(int i = 0; i < (int)(SystemSettings::GUICONFIGDATA_NUMELEM); i++) { + configData.UAVObject[i] = 0; //systemSettingsData.GUIConfigData[i]; + } + + return configData; +} + +void VehicleConfigurationHelper::setupQuadCopter() +{ + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + SystemSettings::AirframeTypeOptions frame; + + switch(m_configSource->getVehicleSubType()) + { + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: { + frame = SystemSettings::AIRFRAMETYPE_QUADP; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 0; + channels[1].yaw = 50; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = -50; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 100; + channels[3].pitch = 0; + channels[3].yaw = 50; + + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.VTOLMotorW = 4; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: { + frame = SystemSettings::AIRFRAMETYPE_QUADX; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 50; + channels[1].yaw = 50; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -50; + channels[2].yaw = -50; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 50; + channels[3].pitch = -50; + channels[3].yaw = 50; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; + + break; + } + default: + break; + } + applyMixerConfiguration(channels); + applyMultiGUISettings(frame, guiSettings); +} + +void VehicleConfigurationHelper::setupHexaCopter() +{ + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + SystemSettings::AirframeTypeOptions frame; + + switch(m_configSource->getVehicleSubType()) + { + case VehicleConfigurationSource::MULTI_ROTOR_HEXA: { + frame = SystemSettings::AIRFRAMETYPE_HEXA; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -33; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -50; + channels[1].pitch = 33; + channels[1].yaw = 33; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = -33; + channels[2].yaw = -33; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = -33; + channels[3].yaw = 33; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 50; + channels[4].pitch = -33; + channels[4].yaw = -33; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 50; + channels[5].pitch = 33; + channels[5].yaw = 33; + + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorS = 4; + guiSettings.multi.VTOLMotorSW = 5; + guiSettings.multi.VTOLMotorNW = 6; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: { + frame = SystemSettings::AIRFRAMETYPE_HEXACOAX; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 25; + channels[0].yaw = -66; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 100; + channels[1].pitch = 25; + channels[1].yaw = 66; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 25; + channels[2].yaw = -66; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 25; + channels[3].yaw = 66; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -50; + channels[4].yaw = -66; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -50; + channels[5].yaw = 66; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorW = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSE = 6; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: { + frame = SystemSettings::AIRFRAMETYPE_HEXAX; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -33; + channels[0].pitch = 50; + channels[0].yaw = -33; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 0; + channels[1].yaw = 33; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = -50; + channels[2].yaw = -33; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -50; + channels[3].yaw = 33; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 33; + channels[4].pitch = 0; + channels[4].yaw = -33; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = 50; + channels[5].yaw = -33; + + guiSettings.multi.VTOLMotorNE = 1; + guiSettings.multi.VTOLMotorE = 2; + guiSettings.multi.VTOLMotorSE = 3; + guiSettings.multi.VTOLMotorSW = 4; + guiSettings.multi.VTOLMotorW = 5; + guiSettings.multi.VTOLMotorNW = 6; + + break; + } + default: + break; + } + applyMixerConfiguration(channels); + applyMultiGUISettings(frame, guiSettings); +} + +void VehicleConfigurationHelper::setupOctoCopter() +{ + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + SystemSettings::AirframeTypeOptions frame; + + switch(m_configSource->getVehicleSubType()) + { + case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { + frame = SystemSettings::AIRFRAMETYPE_OCTO; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 33; + channels[0].yaw = -25; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -33; + channels[1].pitch = 33; + channels[1].yaw = 25; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -33; + channels[2].pitch = 0; + channels[2].yaw = -25; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -33; + channels[3].pitch = -33; + channels[3].yaw = 25; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -33; + channels[4].yaw = -25; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 33; + channels[5].pitch = -33; + channels[5].yaw = 25; + + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 33; + channels[6].pitch = 0; + channels[6].yaw = -25; + + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 33; + channels[7].pitch = 33; + channels[7].yaw = 25; + + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 50; + channels[0].pitch = 50; + channels[0].yaw = -50; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 50; + channels[1].pitch = 50; + channels[1].yaw = 50; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -50; + channels[2].pitch = 50; + channels[2].yaw = -50; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -50; + channels[3].pitch = 50; + channels[3].yaw = 50; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = -50; + channels[4].pitch = -50; + channels[4].yaw = -50; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = -50; + channels[5].pitch = -50; + channels[5].yaw = 50; + + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 50; + channels[6].pitch = -50; + channels[6].yaw = -50; + + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 50; + channels[7].pitch = -50; + channels[7].yaw = 50; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorN = 2; + guiSettings.multi.VTOLMotorNE = 3; + guiSettings.multi.VTOLMotorE = 4; + guiSettings.multi.VTOLMotorSE = 5; + guiSettings.multi.VTOLMotorS = 6; + guiSettings.multi.VTOLMotorSW = 7; + guiSettings.multi.VTOLMotorW = 8; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: { + frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXP; + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 0; + channels[0].pitch = 100; + channels[0].yaw = -50; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = 0; + channels[1].pitch = 100; + channels[1].yaw = 50; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = 0; + channels[2].yaw = -50; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -100; + channels[3].pitch = 0; + channels[3].yaw = 50; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = -100; + channels[4].yaw = -50; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 0; + channels[5].pitch = -100; + channels[5].yaw = 50; + + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 100; + channels[6].pitch = 0; + channels[6].yaw = -50; + + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 100; + channels[7].pitch = 0; + channels[7].yaw = 50; + + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; + + break; + } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: { + frame = SystemSettings::AIRFRAMETYPE_OCTOV; + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -25; + channels[0].pitch = 8; + channels[0].yaw = -25; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -25; + channels[1].pitch = 25; + channels[1].yaw = 25; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -25; + channels[2].pitch = -25; + channels[2].yaw = -25; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -25; + channels[3].pitch = -8; + channels[3].yaw = 25; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 25; + channels[4].pitch = -8; + channels[4].yaw = -25; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 25; + channels[5].pitch = -25; + channels[5].yaw = 25; + + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 25; + channels[6].pitch = 25; + channels[6].yaw = -25; + + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 25; + channels[7].pitch = 8; + channels[7].yaw = 25; + + guiSettings.multi.VTOLMotorN = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorE = 3; + guiSettings.multi.VTOLMotorSE = 4; + guiSettings.multi.VTOLMotorS = 5; + guiSettings.multi.VTOLMotorSW = 6; + guiSettings.multi.VTOLMotorW = 7; + guiSettings.multi.VTOLMotorNW = 8; + + break; + } + default: + break; + } + + applyMixerConfiguration(channels); + applyMultiGUISettings(frame, guiSettings); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h new file mode 100644 index 000000000..44f2125b1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * + * @file vehicleconfigurationhelper.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehicleConfigurationHelper + * @{ + * @brief + *****************************************************************************/ +/* + * 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 VEHICLECONFIGURATIONHELPER_H +#define VEHICLECONFIGURATIONHELPER_H + +#include +#include +#include "vehicleconfigurationsource.h" +#include "uavobjectmanager.h" +#include "systemsettings.h" +#include "cfg_vehicletypes/vehicleconfig.h" +#include "actuatorsettings.h" + +struct mixerChannelSettings { + int type; + int throttle1; + int throttle2; + int roll; + int pitch; + int yaw; + + mixerChannelSettings() : type(), throttle1(), throttle2(), roll(), pitch(), yaw() {} + + mixerChannelSettings(int t, int th1, int th2, int r, int p, int y) + : type(t), throttle1(th1), throttle2(th2), roll(r), pitch(p), yaw(y) {} +}; + +class VehicleConfigurationHelper : public QObject +{ + Q_OBJECT + +public: + VehicleConfigurationHelper(VehicleConfigurationSource* configSource); + bool setupVehicle(bool save = true); + bool setupHardwareSettings(bool save = true); + static const qint16 LEGACY_ESC_FREQUENCE; + static const qint16 RAPID_ESC_FREQUENCE; + +signals: + void saveProgress(int total, int current, QString description); + +private: + static const int MIXER_TYPE_DISABLED = 0; + static const int MIXER_TYPE_MOTOR = 1; + static const int MIXER_TYPE_SERVO = 2; + + VehicleConfigurationSource *m_configSource; + UAVObjectManager *m_uavoManager; + + QList* > m_modifiedObjects; + void addModifiedObject(UAVDataObject* object, QString description); + void clearModifiedObjects(); + + void applyHardwareConfiguration(); + void applyVehicleConfiguration(); + void applyActuatorConfiguration(); + void applyFlighModeConfiguration(); + void applyLevellingConfiguration(); + void applyStabilizationConfiguration(); + void applyManualControlDefaults(); + + void applyMixerConfiguration(mixerChannelSettings channels[]); + + GUIConfigDataUnion getGUIConfigData(); + void applyMultiGUISettings(SystemSettings::AirframeTypeOptions airframe, GUIConfigDataUnion guiConfig); + + bool saveChangesToController(bool save); + QEventLoop m_eventLoop; + bool m_transactionOK; + bool m_transactionTimeout; + int m_currentTransactionObjectID; + int m_progress; + + void resetVehicleConfig(); + void resetGUIData(); + + void setupTriCopter(); + void setupQuadCopter(); + void setupHexaCopter(); + void setupOctoCopter(); + +private slots: + void uAVOTransactionCompleted(UAVObject* object, bool success); + void uAVOTransactionCompleted(int oid, bool success); + void saveChangesTimeout(); + + +}; + +#endif // VEHICLECONFIGURATIONHELPER_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp new file mode 100644 index 000000000..3acb2400d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.cpp @@ -0,0 +1,32 @@ +/** + ****************************************************************************** + * + * @file vehicleconfigurationsource.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehicleConfigurationSource + * @{ + * @brief + *****************************************************************************/ +/* + * 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 "vehicleconfigurationsource.h" + +VehicleConfigurationSource::VehicleConfigurationSource() +{ +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h new file mode 100644 index 000000000..5e36902e9 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -0,0 +1,85 @@ +/** + ****************************************************************************** + * + * @file vehicleconfigurationsource.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup VehicleConfigurationSource + * @{ + * @brief + *****************************************************************************/ +/* + * 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 VEHICLECONFIGURATIONSOURCE_H +#define VEHICLECONFIGURATIONSOURCE_H + +#include +#include "actuatorsettings.h" + +struct accelGyroBias { + float m_accelerometerXBias; + float m_accelerometerYBias; + float m_accelerometerZBias; + + float m_gyroXBias; + float m_gyroYBias; + float m_gyroZBias; +}; + +struct actuatorChannelSettings { + quint16 channelMin; + quint16 channelNeutral; + quint16 channelMax; + + //Default values + actuatorChannelSettings(): channelMin(1000), channelNeutral(1000), channelMax(1900) {} +}; + + +class VehicleConfigurationSource +{ +public: + VehicleConfigurationSource(); + + enum CONTROLLER_TYPE {CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_PIPX}; + enum VEHICLE_TYPE {VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE}; + enum VEHICLE_SUB_TYPE {MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, + MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, + MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, + FIXED_WING_VTAIL, HELI_CCPM}; + enum ESC_TYPE {ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN}; + enum INPUT_TYPE {INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN}; + + virtual VehicleConfigurationSource::CONTROLLER_TYPE getControllerType() const = 0; + virtual VehicleConfigurationSource::VEHICLE_TYPE getVehicleType() const = 0; + virtual VehicleConfigurationSource::VEHICLE_SUB_TYPE getVehicleSubType() const = 0; + virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; + virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; + + virtual bool isLevellingPerformed() const = 0; + virtual accelGyroBias getLevellingBias() const = 0; + + virtual bool isMotorCalibrationPerformed() const = 0; + virtual QList getActuatorSettings() const = 0; + + virtual bool isRestartNeeded() const = 0; + + virtual QString getSummaryText() = 0; +}; + +#endif // VEHICLECONFIGURATIONSOURCE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc new file mode 100644 index 000000000..342af58e5 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc @@ -0,0 +1,38 @@ + + + resources/bttn-heli-down.png + resources/bttn-heli-over.png + resources/bttn-heli-up.png + resources/bttn-land-down.png + resources/bttn-land-over.png + resources/bttn-land-up.png + resources/bttn-multi-down.png + resources/bttn-multi-over.png + resources/bttn-multi-up.png + resources/bttn-plane-down.png + resources/bttn-plane-over.png + resources/bttn-plane-up.png + resources/bttn-ESC-down.png + resources/bttn-ESC-up.png + resources/bttn-ppm-down.png + resources/bttn-ppm-up.png + resources/bttn-pwm-down.png + resources/bttn-pwm-up.png + resources/bttn-sat-down.png + resources/bttn-sat-up.png + resources/bttn-sbus-down.png + resources/bttn-sbus-up.png + resources/bttn-txwizard-off.png + resources/bttn-txwizard-on.png + resources/connection-diagrams.svg + resources/bttn-calculate-down.png + resources/bttn-calculate-up.png + resources/bttn-turbo-down.png + resources/bttn-turbo-up.png + resources/multirotor-shapes.svg + resources/bttn-illustration-down.png + resources/bttn-illustration-up.png + resources/bttn-save-down.png + resources/bttn-save-up.png + + diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui index 350fc440b..3b5cff751 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui @@ -118,7 +118,7 @@ - Only hilight nodes when value actually changes + Only highlight nodes when value actually changes diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png index c4e77c025..ee096b76e 100644 Binary files a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png and b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc.png differ diff --git a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png index b9ad6fa4f..69fd4aa8c 100644 Binary files a/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png and b/ground/openpilotgcs/src/plugins/uploader/images/gcs-board-cc3d.png differ diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 314914667..06efb17b1 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -71,8 +71,6 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) onAutopilotConnect(); versionMatchCheck(); } - - } @@ -687,7 +685,8 @@ void UploaderGadgetWidget::versionMatchCheck() "GCS version: %1 Firmware version: %2.")).arg(gcsVersion).arg(fwVersion); msg->showMessage(warning); } - } +} + void UploaderGadgetWidget::openHelp() { diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/WelcomePageButton.qml b/ground/openpilotgcs/src/plugins/welcome/qml/WelcomePageButton.qml index 4874022be..9e5c1a74f 100644 --- a/ground/openpilotgcs/src/plugins/welcome/qml/WelcomePageButton.qml +++ b/ground/openpilotgcs/src/plugins/welcome/qml/WelcomePageButton.qml @@ -4,7 +4,7 @@ import QtQuick 1.1 Item { id: welcomeButton width: Math.max(116, icon.width) - height: 116 + height: icon.height z: 0 property string baseIconName diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-off.png b/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-off.png new file mode 100644 index 000000000..6e00e1526 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-off.png differ diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-on.png b/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-on.png new file mode 100644 index 000000000..0f85087b3 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/welcome/qml/images/wizard-on.png differ diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/main.qml b/ground/openpilotgcs/src/plugins/welcome/qml/main.qml index f2f2a302c..dc88c42bc 100644 --- a/ground/openpilotgcs/src/plugins/welcome/qml/main.qml +++ b/ground/openpilotgcs/src/plugins/welcome/qml/main.qml @@ -24,27 +24,6 @@ Rectangle { smooth: true } - Column { - id: wizarButtonsColumn - - anchors { - top: parent.top - right: parent.right - margins: 8 - } - spacing: 8 - - WelcomePageButton { - baseIconName: "bttn-vehwizard" - onClicked: welcomePlugin.openPage("VehWizard") - } - - WelcomePageButton { - baseIconName: "bttn-txwizard" - onClicked: welcomePlugin.openPage("TxWizard") - } - } - Column { id: buttonsGrid @@ -58,8 +37,7 @@ Rectangle { Row { //if the buttons grid overlaps vertically with the wizard buttons, //move it left to use only the space left to wizard buttons - property real availableWidth: buttonsGrid.y > wizarButtonsColumn.y+wizarButtonsColumn.height ? - container.width : wizarButtonsColumn.x + property real availableWidth: container.width x: (availableWidth-width)/2 spacing: 16 @@ -69,7 +47,7 @@ Rectangle { anchors.verticalCenterOffset: -2 //it looks better aligned to icons grid //hide the logo on the very small screen to fit the buttons - visible: parent.availableWidth > width + parent.spacing + buttons.width + visible: parent.availableWidth > width + parent.spacing + buttons.width + wizard.width } Grid { @@ -96,7 +74,7 @@ Rectangle { onClicked: welcomePlugin.openPage("System") } - WelcomePageButton { + WelcomePageButton { baseIconName: "scopes" label: "Scopes" onClicked: welcomePlugin.openPage("Scopes") @@ -114,6 +92,14 @@ Rectangle { onClicked: welcomePlugin.openPage("Firmware") } } //icons grid + + WelcomePageButton { + id: wizard + anchors.verticalCenter: parent.verticalCenter + baseIconName: "wizard" + onClicked: welcomePlugin.triggerAction("SetupWizardPlugin.ShowSetupWizard") + } + } // images row CommunityPanel { diff --git a/ground/openpilotgcs/src/plugins/welcome/welcome.qrc b/ground/openpilotgcs/src/plugins/welcome/welcome.qrc index 2fc8bf23c..5007edc6b 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcome.qrc +++ b/ground/openpilotgcs/src/plugins/welcome/welcome.qrc @@ -30,5 +30,7 @@ qml/images/bttn-txwizard-off.png qml/images/bttn-vehwizard-on.png qml/images/bttn-vehwizard-off.png + qml/images/wizard-off.png + qml/images/wizard-on.png diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp index cf36b3036..a109f328a 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.cpp @@ -92,7 +92,7 @@ QString WelcomeMode::name() const QIcon WelcomeMode::icon() const { - return QIcon(QLatin1String(":/core/images/openpilot_logo_64.png")); + return QIcon(QLatin1String(":/core/images/openpiloticon.png")); } int WelcomeMode::priority() const @@ -127,4 +127,9 @@ void WelcomeMode::openPage(const QString &page) Core::ModeManager::instance()->activateModeByWorkspaceName(page); } +void WelcomeMode::triggerAction(const QString &actionId) +{ + Core::ModeManager::instance()->triggerAction(actionId); +} + } // namespace Welcome diff --git a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h index 24b47613e..c1b259552 100644 --- a/ground/openpilotgcs/src/plugins/welcome/welcomemode.h +++ b/ground/openpilotgcs/src/plugins/welcome/welcomemode.h @@ -65,6 +65,7 @@ public: public slots: void openUrl(const QString &url); void openPage(const QString &page); + void triggerAction(const QString &actionId); private: WelcomeModePrivate *m_d; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index b225c6715..113f059fe 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,7 +1,7 @@ Selection of optional hardware configurations. - +