Merge branch 'next' into cp/extraGUIchanges
Conflicts: ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.ui ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui
@ -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
|
||||
|
@ -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
|
||||
|
||||
|
@ -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:
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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)
|
||||
|
@ -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 */
|
||||
|
@ -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 */
|
||||
|
@ -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 */
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
@ -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 */
|
||||
|
@ -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")));
|
||||
|
||||
|
@ -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")));
|
||||
|
||||
|
@ -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 */
|
||||
|
||||
|
@ -36,7 +36,7 @@
|
||||
#include <pios_stm32.h>
|
||||
|
||||
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;
|
||||
|
@ -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 */
|
||||
|
||||
/**
|
||||
|
@ -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 */
|
||||
|
||||
|
@ -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 */
|
||||
|
||||
|
@ -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)
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
@ -8,6 +8,8 @@
|
||||
<ExpertMode>false</ExpertMode>
|
||||
<OverrideLanguage>en_AU</OverrideLanguage>
|
||||
<SaveSettingsOnExit>true</SaveSettingsOnExit>
|
||||
<SettingsWindowHeight>700</SettingsWindowHeight>
|
||||
<SettingsWindowWidth>800</SettingsWindowWidth>
|
||||
<StyleSheet>default</StyleSheet>
|
||||
<UDPMirror>false</UDPMirror>
|
||||
</General>
|
||||
@ -1683,7 +1685,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>mapquad.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</Google__PCT__20Sat>
|
||||
<Memory__PCT__20Only>
|
||||
@ -1703,7 +1705,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>airplanepip.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</Memory__PCT__20Only>
|
||||
<default>
|
||||
@ -1723,7 +1725,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>mapquad.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</default>
|
||||
</OPMapGadget>
|
||||
@ -2636,7 +2638,7 @@
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>2</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAClQAAAAIAAAB3)</splitterSizes>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAABhAAAAAIAAAGE)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
@ -2669,8 +2671,20 @@
|
||||
</side0>
|
||||
<side1>
|
||||
<side0>
|
||||
<classId>EmptyGadget</classId>
|
||||
<type>uavGadget</type>
|
||||
<side0>
|
||||
<classId>PfdQmlGadget</classId>
|
||||
<gadget>
|
||||
<activeConfiguration>NoTerrain</activeConfiguration>
|
||||
</gadget>
|
||||
<type>uavGadget</type>
|
||||
</side0>
|
||||
<side1>
|
||||
<classId>MagicWaypointGadget</classId>
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAB7wAAAAIAAAGU)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side0>
|
||||
<side1>
|
||||
<classId>GCSControlGadget</classId>
|
||||
@ -2680,7 +2694,7 @@
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAB6AAAAAIAAADC)</splitterSizes>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAADhAAAAAIAAAIX)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side1>
|
||||
<splitterOrientation>2</splitterOrientation>
|
||||
@ -2739,7 +2753,7 @@
|
||||
</UAVGadgetManager>
|
||||
<Workspace>
|
||||
<AllowTabBarMovement>false</AllowTabBarMovement>
|
||||
<Icon1>:\core\images\ah.png</Icon1>
|
||||
<Icon1>:/core/images/ah.png</Icon1>
|
||||
<Icon10>:/core/images/openpilot_logo_64.png</Icon10>
|
||||
<Icon2>:/core/images/config.png</Icon2>
|
||||
<Icon3>:/core/images/cog.png</Icon3>
|
||||
|
@ -6,7 +6,7 @@
|
||||
<LastPreferenceCategory>Notify Plugin</LastPreferenceCategory>
|
||||
<LastPreferencePage>settings</LastPreferencePage>
|
||||
<SaveSettingsOnExit>true</SaveSettingsOnExit>
|
||||
<SettingsWindowHeight>600</SettingsWindowHeight>
|
||||
<SettingsWindowHeight>700</SettingsWindowHeight>
|
||||
<SettingsWindowWidth>800</SettingsWindowWidth>
|
||||
<UDPMirror>false</UDPMirror>
|
||||
<Description>Wide configuration</Description>
|
||||
@ -1679,7 +1679,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>mapquad.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</Google__PCT__20Sat>
|
||||
<Memory__PCT__20Only>
|
||||
@ -1698,7 +1698,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>airplanepip.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</Memory__PCT__20Only>
|
||||
<default>
|
||||
@ -1717,7 +1717,7 @@
|
||||
<showTileGridLines>false</showTileGridLines>
|
||||
<uavSymbol>mapquad.png</uavSymbol>
|
||||
<useMemoryCache>true</useMemoryCache>
|
||||
<useOpenGL>false</useOpenGL>
|
||||
<useOpenGL>true</useOpenGL>
|
||||
</data>
|
||||
</default>
|
||||
</OPMapGadget>
|
||||
@ -2636,7 +2636,7 @@
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>2</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAClQAAAAIAAAB3)</splitterSizes>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAABhAAAAAIAAAGE)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
@ -2669,8 +2669,20 @@
|
||||
</side0>
|
||||
<side1>
|
||||
<side0>
|
||||
<classId>EmptyGadget</classId>
|
||||
<type>uavGadget</type>
|
||||
<side0>
|
||||
<classId>PfdQmlGadget</classId>
|
||||
<gadget>
|
||||
<activeConfiguration>NoTerrain</activeConfiguration>
|
||||
</gadget>
|
||||
<type>uavGadget</type>
|
||||
</side0>
|
||||
<side1>
|
||||
<classId>MagicWaypointGadget</classId>
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAB7wAAAAIAAAGU)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side0>
|
||||
<side1>
|
||||
<classId>GCSControlGadget</classId>
|
||||
@ -2680,7 +2692,7 @@
|
||||
<type>uavGadget</type>
|
||||
</side1>
|
||||
<splitterOrientation>1</splitterOrientation>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAAB6AAAAAIAAADC)</splitterSizes>
|
||||
<splitterSizes>@Variant(AAAACQAAAAIAAAACAAADhAAAAAIAAAIX)</splitterSizes>
|
||||
<type>splitter</type>
|
||||
</side1>
|
||||
<splitterOrientation>2</splitterOrientation>
|
||||
@ -2739,7 +2751,7 @@
|
||||
</UAVGadgetManager>
|
||||
<Workspace>
|
||||
<AllowTabBarMovement>false</AllowTabBarMovement>
|
||||
<Icon1>:\core\images\ah.png</Icon1>
|
||||
<Icon1>:/core/images/ah.png</Icon1>
|
||||
<Icon10>:/core/images/openpilot_logo_64.png</Icon10>
|
||||
<Icon2>:/core/images/config.png</Icon2>
|
||||
<Icon3>:/core/images/cog.png</Icon3>
|
||||
|
@ -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
|
||||
|
@ -43,8 +43,6 @@ namespace core {
|
||||
LanguageStr=LanguageType().toShortString(Language);
|
||||
Cache::Instance();
|
||||
|
||||
// OPMaps::MemoryCache();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
@ -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
|
||||
{
|
||||
|
@ -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
|
||||
{
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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);
|
||||
|
@ -1,4 +1,4 @@
|
||||
<plugin name="Config" version="0.0.1" compatVersion="1.0.0">
|
||||
<plugin name="Config" version="1.0.0" compatVersion="1.0.0">
|
||||
<vendor>The OpenPilot Project</vendor>
|
||||
<copyright>(C) 2010 OpenPilot Project</copyright>
|
||||
<license>GNU Public License (GPL) Version 3</license>
|
||||
|
@ -1,23 +1,32 @@
|
||||
<RCC>
|
||||
<qresource prefix="/configgadget">
|
||||
<file>images/help2.png</file>
|
||||
<file>images/Airframe.png</file>
|
||||
<file>images/Servo.png</file>
|
||||
<file>images/ahrs-calib.svg</file>
|
||||
<file>images/AHRS-v1.3.png</file>
|
||||
<file>images/paper-plane.svg</file>
|
||||
<file>images/curve-bg.svg</file>
|
||||
<file>images/multirotor-shapes.svg</file>
|
||||
<file>images/ccpm_setup.svg</file>
|
||||
<file>images/PipXtreme.png</file>
|
||||
<file>images/Transmitter.png</file>
|
||||
<file>images/help.png</file>
|
||||
<file>images/coptercontrol.svg</file>
|
||||
<file>images/hw_config.png</file>
|
||||
<file>images/gyroscope.png</file>
|
||||
<file>images/TX.svg</file>
|
||||
<file>images/TX2.svg</file>
|
||||
<file>images/camera.png</file>
|
||||
<file>images/txpid.png</file>
|
||||
<file>images/output_selected.png</file>
|
||||
<file>images/output_normal.png</file>
|
||||
<file>images/input_selected.png</file>
|
||||
<file>images/input_normal.png</file>
|
||||
<file>images/hardware_normal.png</file>
|
||||
<file>images/hardware_selected.png</file>
|
||||
<file>images/vehicle_normal.png</file>
|
||||
<file>images/vehicle_selected.png</file>
|
||||
<file>images/ins_selected.png</file>
|
||||
<file>images/ins_normal.png</file>
|
||||
<file>images/stabilization_selected.png</file>
|
||||
<file>images/stabilization_normal.png</file>
|
||||
<file>images/autotune_selected.png</file>
|
||||
<file>images/autotune_normal.png</file>
|
||||
<file>images/txpid_selected.png</file>
|
||||
<file>images/txpid_normal.png</file>
|
||||
<file>images/camstab_selected.png</file>
|
||||
<file>images/camstab_normal.png</file>
|
||||
</qresource>
|
||||
</RCC>
|
||||
|
@ -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 <coreplugin/iuavgadget.h>
|
||||
#include <coreplugin/coreconstants.h>
|
||||
#include <coreplugin/actionmanager/actionmanager.h>
|
||||
#include <coreplugin/icore.h>
|
||||
#include <coreplugin/modemanager.h>
|
||||
|
||||
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<int>() <<
|
||||
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<ConfigGadgetConfiguration*>(config));
|
||||
}
|
||||
|
||||
void ConfigGadgetFactory::startInputWizard()
|
||||
{
|
||||
if(gadgetWidget)
|
||||
{
|
||||
Core::ModeManager::instance()->activateModeByWorkspaceName("Configuration");
|
||||
gadgetWidget->startInputWizard();
|
||||
}
|
||||
}
|
||||
|
@ -28,6 +28,9 @@
|
||||
#define CONFIGGADGETFACTORY_H
|
||||
|
||||
#include <coreplugin/iuavgadgetfactory.h>
|
||||
#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
|
||||
|
@ -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<ConfigInputWidget*>(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<ConfigTaskWidget *>(ftw->currentWidget());
|
||||
if(!wid)
|
||||
*proceed = true;
|
||||
ConfigTaskWidget * wid = qobject_cast<ConfigTaskWidget *>(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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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();
|
||||
|
2707
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
Executable file → Normal file
@ -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 <QDebug>
|
||||
#include <QStringList>
|
||||
#include <QtGui/QWidget>
|
||||
#include <QtGui/QTextEdit>
|
||||
#include <QtGui/QVBoxLayout>
|
||||
#include <QtGui/QPushButton>
|
||||
#include <QDesktopServices>
|
||||
#include <QUrl>
|
||||
#include <QMessageBox>
|
||||
#include <utils/stylehelper.h>
|
||||
#include <QMessageBox>
|
||||
|
||||
#include <extensionsystem/pluginmanager.h>
|
||||
#include <coreplugin/generalsettings.h>
|
||||
|
||||
#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<Core::Internal::GeneralSettings>();
|
||||
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<QWidget> 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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
// Preserve the inverted status
|
||||
if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) {
|
||||
manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i];
|
||||
manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i];
|
||||
} else {
|
||||
// Make this detect as still inverted
|
||||
manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i] + 1;
|
||||
manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i];
|
||||
}
|
||||
}
|
||||
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits()));
|
||||
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
}
|
||||
break;
|
||||
case wizardIdentifyInverted:
|
||||
dimOtherControls(true);
|
||||
setTxMovement(nothing);
|
||||
extraWidgets.clear();
|
||||
|
||||
for (int index = 0; index < manualSettingsObj->getField("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<QRadioButton *>(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<QRadioButton *>(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;i<ManualControlCommand::CHANNEL_NUMELEM;++i)
|
||||
{
|
||||
manualSettingsData.ChannelNeutral[i]=manualCommandData.Channel[i];
|
||||
}
|
||||
manualSettingsObj->setData(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<QCheckBox *>(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 <int> 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 <int> 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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) {
|
||||
// Non inverted channel
|
||||
if(manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
if(manualSettingsData.ChannelMax[i]<manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i];
|
||||
} else {
|
||||
// Inverted channel
|
||||
if(manualSettingsData.ChannelMax[i]>manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i];
|
||||
if(manualSettingsData.ChannelMin[i]<manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
}
|
||||
}
|
||||
manualSettingsObj->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(movePos<limitMin)
|
||||
{
|
||||
movePos=movePos+2;
|
||||
growing=true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigInputWidget::moveSticks()
|
||||
{
|
||||
QTransform trans;
|
||||
manualCommandData=manualCommandObj->getData();
|
||||
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<QCheckBox *>(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]<manualSettingsData.ChannelMin[index])))
|
||||
{
|
||||
qint16 aux;
|
||||
aux=manualSettingsData.ChannelMax[index];
|
||||
manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index];
|
||||
manualSettingsData.ChannelMin[index]=aux;
|
||||
}
|
||||
}
|
||||
}
|
||||
manualSettingsObj->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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
if((!reverse[i] && manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i]) ||
|
||||
(reverse[i] && manualSettingsData.ChannelMin[i]<manualCommandData.Channel[i]))
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
if((!reverse[i] && manualSettingsData.ChannelMax[i]<manualCommandData.Channel[i]) ||
|
||||
(reverse[i] && manualSettingsData.ChannelMax[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 <QDebug>
|
||||
#include <QStringList>
|
||||
#include <QtGui/QWidget>
|
||||
#include <QtGui/QTextEdit>
|
||||
#include <QtGui/QVBoxLayout>
|
||||
#include <QtGui/QPushButton>
|
||||
#include <QDesktopServices>
|
||||
#include <QUrl>
|
||||
#include <QMessageBox>
|
||||
#include <utils/stylehelper.h>
|
||||
#include <QMessageBox>
|
||||
|
||||
#include <extensionsystem/pluginmanager.h>
|
||||
#include <coreplugin/generalsettings.h>
|
||||
|
||||
#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<Core::Internal::GeneralSettings>();
|
||||
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<QWidget> 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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
// Preserve the inverted status
|
||||
if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) {
|
||||
manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i];
|
||||
manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i];
|
||||
} else {
|
||||
// Make this detect as still inverted
|
||||
manualSettingsData.ChannelMin[i]=manualSettingsData.ChannelNeutral[i] + 1;
|
||||
manualSettingsData.ChannelMax[i]=manualSettingsData.ChannelNeutral[i];
|
||||
}
|
||||
}
|
||||
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits()));
|
||||
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks()));
|
||||
}
|
||||
break;
|
||||
case wizardIdentifyInverted:
|
||||
dimOtherControls(true);
|
||||
setTxMovement(nothing);
|
||||
extraWidgets.clear();
|
||||
|
||||
for (int index = 0; index < manualSettingsObj->getField("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<QRadioButton *>(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<QRadioButton *>(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;i<ManualControlCommand::CHANNEL_NUMELEM;++i)
|
||||
{
|
||||
manualSettingsData.ChannelNeutral[i]=manualCommandData.Channel[i];
|
||||
}
|
||||
manualSettingsObj->setData(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<QCheckBox *>(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 <int> 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 <int> 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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
if(manualSettingsData.ChannelMin[i] <= manualSettingsData.ChannelMax[i]) {
|
||||
// Non inverted channel
|
||||
if(manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
if(manualSettingsData.ChannelMax[i]<manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i];
|
||||
} else {
|
||||
// Inverted channel
|
||||
if(manualSettingsData.ChannelMax[i]>manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i];
|
||||
if(manualSettingsData.ChannelMin[i]<manualCommandData.Channel[i])
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
}
|
||||
}
|
||||
manualSettingsObj->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(movePos<limitMin)
|
||||
{
|
||||
movePos=movePos+2;
|
||||
growing=true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ConfigInputWidget::moveSticks()
|
||||
{
|
||||
QTransform trans;
|
||||
manualCommandData=manualCommandObj->getData();
|
||||
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<QCheckBox *>(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]<manualSettingsData.ChannelMin[index])))
|
||||
{
|
||||
qint16 aux;
|
||||
aux=manualSettingsData.ChannelMax[index];
|
||||
manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index];
|
||||
manualSettingsData.ChannelMin[index]=aux;
|
||||
}
|
||||
}
|
||||
}
|
||||
manualSettingsObj->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;i<ManualControlSettings::CHANNELMAX_NUMELEM;++i)
|
||||
{
|
||||
if((!reverse[i] && manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i]) ||
|
||||
(reverse[i] && manualSettingsData.ChannelMin[i]<manualCommandData.Channel[i]))
|
||||
manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i];
|
||||
if((!reverse[i] && manualSettingsData.ChannelMax[i]<manualCommandData.Channel[i]) ||
|
||||
(reverse[i] && manualSettingsData.ChannelMax[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()));
|
||||
}
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -99,7 +99,7 @@ void ConfigPlugin::extensionsInitialized()
|
||||
|
||||
void ConfigPlugin::shutdown()
|
||||
{
|
||||
// Do nothing
|
||||
// Do nothing
|
||||
}
|
||||
|
||||
/**
|
||||
|
After Width: | Height: | Size: 33 KiB |
After Width: | Height: | Size: 34 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/camstab_normal.png
Normal file
After Width: | Height: | Size: 34 KiB |
After Width: | Height: | Size: 35 KiB |
After Width: | Height: | Size: 26 KiB |
After Width: | Height: | Size: 24 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/input_normal.png
Normal file
After Width: | Height: | Size: 34 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/input_selected.png
Normal file
After Width: | Height: | Size: 36 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/ins_normal.png
Normal file
After Width: | Height: | Size: 38 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/ins_selected.png
Normal file
After Width: | Height: | Size: 40 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/output_normal.png
Normal file
After Width: | Height: | Size: 20 KiB |
After Width: | Height: | Size: 20 KiB |
After Width: | Height: | Size: 53 KiB |
After Width: | Height: | Size: 58 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/txpid_normal.png
Normal file
After Width: | Height: | Size: 32 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/txpid_selected.png
Normal file
After Width: | Height: | Size: 34 KiB |
BIN
ground/openpilotgcs/src/plugins/config/images/vehicle_normal.png
Normal file
After Width: | Height: | Size: 27 KiB |
After Width: | Height: | Size: 32 KiB |
@ -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";
|
||||
}
|
||||
}
|
||||
|
@ -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<DevListItem> 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<Core::DevListItem> devices);
|
||||
|
||||
public slots:
|
||||
void telemetryConnected();
|
||||
|
@ -39,7 +39,6 @@
|
||||
#include <QtCore/QSettings>
|
||||
|
||||
#include "ui_generalsettings.h"
|
||||
#include <QKeyEvent>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
@ -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<QTextCodec *> m_codecs;
|
||||
|
||||
};
|
||||
class globalSettingsWidget:public QWidget
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
globalSettingsWidget(QWidget * parent);
|
||||
protected:
|
||||
void keyPressEvent(QKeyEvent *);
|
||||
signals:
|
||||
void showHidden();
|
||||
};
|
||||
|
||||
} // namespace Internal
|
||||
} // namespace Core
|
||||
|
||||
|
Before Width: | Height: | Size: 991 B After Width: | Height: | Size: 1.8 KiB |
Before Width: | Height: | Size: 512 B After Width: | Height: | Size: 1.3 KiB |
Before Width: | Height: | Size: 610 B After Width: | Height: | Size: 1.2 KiB |
Before Width: | Height: | Size: 559 B After Width: | Height: | Size: 1.5 KiB |
Before Width: | Height: | Size: 710 B After Width: | Height: | Size: 1.2 KiB |
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -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);
|
||||
|
@ -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();
|
||||
|
@ -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++) {
|
||||
|
@ -6,161 +6,158 @@
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>575</width>
|
||||
<height>551</height>
|
||||
<width>615</width>
|
||||
<height>577</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>Form</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_8">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<item>
|
||||
<widget class="QScrollArea" name="scrollArea">
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::NoFrame</enum>
|
||||
</property>
|
||||
<property name="frameShadow">
|
||||
<enum>QFrame::Plain</enum>
|
||||
</property>
|
||||
<property name="widgetResizable">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QWidget" name="scrollAreaWidgetContents_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>560</width>
|
||||
<height>552</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="text">
|
||||
<string>Choose flight simulator:</string>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="text">
|
||||
<string>Choose flight simulator:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="chooseFlightSimulator"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_6">
|
||||
<property name="text">
|
||||
<string>Local interface (IP):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLineEdit" name="hostAddress">
|
||||
<property name="toolTip">
|
||||
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_9">
|
||||
<property name="text">
|
||||
<string>Remote interface (IP):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLineEdit" name="remoteAddress">
|
||||
<property name="toolTip">
|
||||
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="text">
|
||||
<string>Input Port:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="3">
|
||||
<widget class="QLineEdit" name="inputPort">
|
||||
<property name="toolTip">
|
||||
<string>For receiving data from sim</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="text">
|
||||
<string>Output Port:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<widget class="QLineEdit" name="outputPort">
|
||||
<property name="toolTip">
|
||||
<string>For sending data to sim</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout">
|
||||
<property name="fieldGrowthPolicy">
|
||||
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
|
||||
</property>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Path executable:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="Utils::PathChooser" name="executablePath" native="true"/>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Data directory:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="Utils::PathChooser" name="dataPath" native="true"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_3">
|
||||
<property name="topMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>6</number>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="chooseFlightSimulator"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_6">
|
||||
<property name="text">
|
||||
<string>Local interface (IP):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLineEdit" name="hostAddress">
|
||||
<property name="toolTip">
|
||||
<string>For communication with sim computer via network. Should be the IP address of one of the interfaces of the GCS computer.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_9">
|
||||
<property name="text">
|
||||
<string>Remote interface (IP):</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLineEdit" name="remoteAddress">
|
||||
<property name="toolTip">
|
||||
<string>Only required if running simulator on remote machine. Should be the IP of the machine on which the simulator is running.</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_5">
|
||||
<property name="text">
|
||||
<string>Input Port:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="3">
|
||||
<widget class="QLineEdit" name="inputPort">
|
||||
<property name="toolTip">
|
||||
<string>For receiving data from sim</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<widget class="QLabel" name="label_4">
|
||||
<property name="text">
|
||||
<string>Output Port:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<widget class="QLineEdit" name="outputPort">
|
||||
<property name="toolTip">
|
||||
<string>For sending data to sim</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout">
|
||||
<property name="fieldGrowthPolicy">
|
||||
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
|
||||
</property>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Path executable:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="Utils::PathChooser" name="executablePath" native="true"/>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Data directory:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="Utils::PathChooser" name="dataPath" native="true"/>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_3">
|
||||
<property name="topMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QScrollArea" name="scrollArea">
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::NoFrame</enum>
|
||||
</property>
|
||||
<property name="frameShadow">
|
||||
<enum>QFrame::Plain</enum>
|
||||
</property>
|
||||
<property name="widgetResizable">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<widget class="QWidget" name="scrollAreaWidgetContents_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>574</width>
|
||||
<height>391</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_4">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QGroupBox" name="groupBox">
|
||||
@ -694,10 +691,10 @@
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
</widget>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
|
@ -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();
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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 <coreplugin/iuavgadget.h>
|
||||
#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 <coreplugin/iuavgadget.h>
|
||||
#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_
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -0,0 +1,12 @@
|
||||
<plugin name="SetupWizard" version="1.0.0" compatVersion="1.0.0">
|
||||
<vendor>The OpenPilot Project</vendor>
|
||||
<copyright>(C) 2012 OpenPilot Project</copyright>
|
||||
<license>The GNU Public License (GPL) Version 3</license>
|
||||
<description>A plugin that provides a setup wizard for easy initial setup of airframes.</description>
|
||||
<url>http://www.openpilot.org</url>
|
||||
<dependencyList>
|
||||
<dependency name="Core" version="1.0.0"/>
|
||||
<dependency name="Config" version="1.0.0"/>
|
||||
<dependency name="UAVObjects" version="1.0.0"/>
|
||||
</dependencyList>
|
||||
</plugin>
|
@ -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 <QDebug>
|
||||
#include <QFile>
|
||||
#include <QFileDialog>
|
||||
#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<QString> 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<QString> 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);
|
||||
}
|
||||
}
|
@ -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 <QDialog>
|
||||
#include <QHash>
|
||||
#include <QSvgRenderer>
|
||||
|
||||
#include <QGraphicsSvgItem>
|
||||
#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<QString> elementsToShow);
|
||||
protected:
|
||||
void resizeEvent(QResizeEvent *event);
|
||||
void showEvent(QShowEvent *event);
|
||||
|
||||
private slots:
|
||||
|
||||
void on_saveButton_clicked();
|
||||
};
|
||||
|
||||
#endif // CONNECTIONDIAGRAM_H
|
112
ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.ui
Normal file
@ -0,0 +1,112 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>ConnectionDiagram</class>
|
||||
<widget class="QDialog" name="ConnectionDiagram">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>800</width>
|
||||
<height>440</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>Dialog</string>
|
||||
</property>
|
||||
<property name="sizeGripEnabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="modal">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<item>
|
||||
<widget class="QGraphicsView" name="connectionDiagram">
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::WinPanel</enum>
|
||||
</property>
|
||||
<property name="verticalScrollBarPolicy">
|
||||
<enum>Qt::ScrollBarAlwaysOff</enum>
|
||||
</property>
|
||||
<property name="horizontalScrollBarPolicy">
|
||||
<enum>Qt::ScrollBarAlwaysOff</enum>
|
||||
</property>
|
||||
<property name="backgroundBrush">
|
||||
<brush brushstyle="SolidPattern">
|
||||
<color alpha="255">
|
||||
<red>255</red>
|
||||
<green>255</green>
|
||||
<blue>255</blue>
|
||||
</color>
|
||||
</brush>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<spacer name="horizontalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="saveButton">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Save</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="closeButton">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Close</string>
|
||||
</property>
|
||||
<property name="default">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<resources/>
|
||||
<connections>
|
||||
<connection>
|
||||
<sender>closeButton</sender>
|
||||
<signal>clicked()</signal>
|
||||
<receiver>ConnectionDiagram</receiver>
|
||||
<slot>close()</slot>
|
||||
<hints>
|
||||
<hint type="sourcelabel">
|
||||
<x>752</x>
|
||||
<y>418</y>
|
||||
</hint>
|
||||
<hint type="destinationlabel">
|
||||
<x>399</x>
|
||||
<y>219</y>
|
||||
</hint>
|
||||
</hints>
|
||||
</connection>
|
||||
</connections>
|
||||
</ui>
|
217
ground/openpilotgcs/src/plugins/setupwizard/levellingutil.cpp
Normal file
@ -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<UAVObjectManager>();
|
||||
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<UAVObjectManager>();
|
||||
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<UAVObjectManager>();
|
||||
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<UAVObjectManager>();
|
||||
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;
|
||||
}
|
||||
|
92
ground/openpilotgcs/src/plugins/setupwizard/levellingutil.h
Normal file
@ -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 <QObject>
|
||||
#include <QTimer>
|
||||
#include <QMutex>
|
||||
|
||||
#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
|
@ -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<UAVObjectManager>();
|
||||
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);
|
||||
}
|
||||
}
|
@ -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 <QObject>
|
||||
#include <QList>
|
||||
#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
|
@ -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);
|
||||
}
|
@ -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 <QWizardPage>
|
||||
#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
|
@ -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 <extensionsystem/pluginmanager.h>
|
||||
#include <uavobjectutil/uavobjectutilmanager.h>
|
||||
|
||||
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<Core::DevListItem>)), this, SLOT(devicesChanged(QLinkedList<Core::DevListItem>)));
|
||||
|
||||
ExtensionSystem::PluginManager *pluginManager = ExtensionSystem::PluginManager::instance();
|
||||
Q_ASSERT(pluginManager);
|
||||
m_telemtryManager = pluginManager->getObject<TelemetryManager>();
|
||||
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<UAVObjectUtilManager>();
|
||||
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("<Unknown>"), 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<Core::DevListItem> 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();
|
||||
}
|
@ -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 <coreplugin/icore.h>
|
||||
#include <coreplugin/connectionmanager.h>
|
||||
#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<Core::DevListItem> devices);
|
||||
void connectionStatusChanged();
|
||||
void connectDisconnect();
|
||||
};
|
||||
|
||||
#endif // CONTROLLERPAGE_H
|
@ -0,0 +1,127 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>ControllerPage</class>
|
||||
<widget class="QWizardPage" name="ControllerPage">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>600</width>
|
||||
<height>400</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>WizardPage</string>
|
||||
</property>
|
||||
<widget class="QLabel" name="label">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>20</x>
|
||||
<y>20</y>
|
||||
<width>550</width>
|
||||
<height>261</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string><!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></string>
|
||||
</property>
|
||||
<property name="textFormat">
|
||||
<enum>Qt::AutoText</enum>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QPushButton" name="connectButton">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>460</x>
|
||||
<y>350</y>
|
||||
<width>100</width>
|
||||
<height>23</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Connect</string>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QLabel" name="label_3">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>40</x>
|
||||
<y>350</y>
|
||||
<width>125</width>
|
||||
<height>16</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>10</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Detected board type:</string>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QComboBox" name="boardTypeCombo">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>170</x>
|
||||
<y>350</y>
|
||||
<width>279</width>
|
||||
<height>22</height>
|
||||
</rect>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QComboBox" name="deviceCombo">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>170</x>
|
||||
<y>310</y>
|
||||
<width>279</width>
|
||||
<height>22</height>
|
||||
</rect>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>40</x>
|
||||
<y>310</y>
|
||||
<width>121</width>
|
||||
<height>16</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>10</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Connection device:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</widget>
|
||||
<resources/>
|
||||
<connections/>
|
||||
</ui>
|
@ -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 <coreplugin/modemanager.h>
|
||||
#include <extensionsystem/pluginmanager.h>
|
||||
#include <configgadgetfactory.h>
|
||||
#include <QMessageBox>
|
||||
|
||||
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<ConfigGadgetFactory>();
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
52
ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.h
Normal file
@ -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
|
88
ground/openpilotgcs/src/plugins/setupwizard/pages/endpage.ui
Normal file
@ -0,0 +1,88 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>EndPage</class>
|
||||
<widget class="QWizardPage" name="EndPage">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>600</width>
|
||||
<height>400</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>600</width>
|
||||
<height>400</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>WizardPage</string>
|
||||
</property>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>20</x>
|
||||
<y>20</y>
|
||||
<width>550</width>
|
||||
<height>251</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string><!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></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QToolButton" name="inputWizardButton">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>200</x>
|
||||
<y>270</y>
|
||||
<width>200</width>
|
||||
<height>100</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">QToolButton { border: none }</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Go to Input Wizard...</string>
|
||||
</property>
|
||||
<property name="icon">
|
||||
<iconset resource="../wizardResources.qrc">
|
||||
<normaloff>:/setupwizard/resources/bttn-txwizard-off.png</normaloff>
|
||||
<normalon>:/setupwizard/resources/bttn-txwizard-on.png</normalon>
|
||||
<activeoff>:/setupwizard/resources/bttn-txwizard-off.png</activeoff>
|
||||
<activeon>:/setupwizard/resources/bttn-txwizard-off.png</activeon>:/setupwizard/resources/bttn-txwizard-off.png</iconset>
|
||||
</property>
|
||||
<property name="iconSize">
|
||||
<size>
|
||||
<width>200</width>
|
||||
<height>100</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="autoRaise">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</widget>
|
||||
<resources>
|
||||
<include location="../wizardResources.qrc"/>
|
||||
</resources>
|
||||
<connections/>
|
||||
</ui>
|
@ -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;
|
||||
}
|
@ -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
|
@ -0,0 +1,43 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<ui version="4.0">
|
||||
<class>FixedWingPage</class>
|
||||
<widget class="QWizardPage" name="FixedWingPage">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>600</width>
|
||||
<height>400</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
<string>WizardPage</string>
|
||||
</property>
|
||||
<widget class="QLabel" name="label_2">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>50</x>
|
||||
<y>160</y>
|
||||
<width>500</width>
|
||||
<height>41</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string><!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></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignHCenter|Qt::AlignTop</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</widget>
|
||||
<resources/>
|
||||
<connections/>
|
||||
</ui>
|
@ -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;
|
||||
}
|