diff --git a/flight/libraries/math/sin_lookup.c b/flight/libraries/math/sin_lookup.c index 6233dc592..9b6bd8ddd 100644 --- a/flight/libraries/math/sin_lookup.c +++ b/flight/libraries/math/sin_lookup.c @@ -105,7 +105,14 @@ float sin_lookup_deg(float angle) return 0; } - int i_ang = ((int32_t)angle) % 360; + // int i_ang = ((int32_t)angle) % 360; + // 1073741760 is a multiple of 360 that is close to 0x3fffffff + // so angle can be a very large number of positive or negative rotations + // this is the fastest fix (no tests, one integer addition) + // and has the largest range since float mantissas are 23-4 bit + // we could halve the lookup table size + // we could interpolate for greater accuracy + int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360; if (i_ang >= 180) { // for 180 to 360 deg return -sin_table[i_ang - 180]; } else { // for 0 to 179 deg diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b10613160..440d02dd9 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -96,7 +96,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont static void updateLandDesired(ManualControlCommandData *cmd, bool changed); static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData *settings, float flightMode); +static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd); static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch); static void setArmedIfChanged(uint8_t val); static void configurationUpdatedCb(UAVObjEvent *ev); @@ -358,6 +358,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } + cmd.FlightModeSwitchPosition = (uint8_t) 255; } else if (valid_input_detected) { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); @@ -446,7 +447,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } - processFlightMode(&settings, flightMode); + processFlightMode(&settings, flightMode, &cmd); } // Process arming outside conditional so system will disarm when disconnected @@ -1198,7 +1199,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData * @param[in] settings The settings which indicate which position is which mode * @param[in] flightMode the value of the switch position */ -static void processFlightMode(ManualControlSettingsData *settings, float flightMode) +static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd) { FlightStatusData flightStatus; @@ -1210,6 +1211,8 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM pos = settings->FlightModeNumber - 1; } + cmd->FlightModeSwitchPosition = pos; + uint8_t newMode = settings->FlightModePosition[pos]; if (flightStatus.FlightMode != newMode) { diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index b2b930c70..c01fb4131 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -48,6 +48,7 @@ #include "airspeedstate.h" #include "gyrostate.h" #include "flightstatus.h" +#include "manualcontrolsettings.h" #include "manualcontrol.h" // Just to get a macro #include "taskinfo.h" @@ -75,6 +76,9 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define FAILSAFE_TIMEOUT_MS 30 +// number of flight mode switch positions +#define NUM_FMS_POSITIONS 6 + // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode // The PID_RATE set is used by the attitude portion of Attitude mode // The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain @@ -98,9 +102,17 @@ bool lowThrottleZeroAxis[MAX_AXES]; float vbar_decay = 0.991f; struct pid pids[PID_MAX]; -int flight_mode = -1; +int cur_flight_mode = -1; static uint8_t rattitude_anti_windup; +static float cruise_control_min_throttle; +static float cruise_control_max_throttle; +static uint8_t cruise_control_max_angle; +static float cruise_control_max_power_factor; +static float cruise_control_power_trim; +static int8_t cruise_control_inverted_power_switch; +static float cruise_control_neutral_thrust; +static uint8_t cruise_control_flight_mode_switch_pos_enable[NUM_FMS_POSITIONS]; // Private functions static void stabilizationTask(void *parameters); @@ -150,6 +162,9 @@ int32_t StabilizationStart() */ int32_t StabilizationInitialize() { + // stop the compile if the number of switch positions changes, but has not been changed here + PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0]) ); + // Initialize variables StabilizationSettingsInitialize(); StabilizationBankInitialize(); @@ -223,9 +238,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) #ifdef DIAG_RATEDESIRED RateDesiredGet(&rateDesired); #endif + uint8_t flight_mode_switch_position; + ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position); - if (flight_mode != flightStatus.FlightMode) { - flight_mode = flightStatus.FlightMode; + if (cur_flight_mode != flight_mode_switch_position) { + cur_flight_mode = flight_mode_switch_position; SettingsBankUpdatedCb(NULL); } @@ -588,6 +605,64 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } } + // modify throttle according to 1/cos(bank angle) + // to maintain same altitdue with changing bank angle + // but without manually adjusting throttle + // do it here and all the various flight modes (e.g. Altitude Hold) can use it + if (flight_mode_switch_position < NUM_FMS_POSITIONS + && cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t) 0 + && cruise_control_max_power_factor > 0.0001f) { + static uint8_t toggle; + static float factor; + float angle; + // get attitude state and calculate angle + // do it every 8th iteration to save CPU + if ((toggle++ & 7) == 0) { + // spherical right triangle + // 0 <= acosf() <= Pi + angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch))); + // if past the cutoff angle (60 to 180 (180 means never)) + if (angle > cruise_control_max_angle) { + // -1 reversed collective, 0 zero power, or 1 normal power + // these are all unboosted + factor = cruise_control_inverted_power_switch; + } else { + // avoid singularity + if (angle > 89.999f && angle < 90.001f) { + factor = cruise_control_max_power_factor; + } else { + factor = 1.0f / fabsf(cos_lookup_deg(angle)); + if (factor > cruise_control_max_power_factor) { + factor = cruise_control_max_power_factor; + } + } + // factor in the power trim, no effect at 1.0, linear effect increases with factor + factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f; + // if inverted and they want negative boost + if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t) -1) { + factor = -factor; + // as long as throttle is getting reversed + // we may as well do pitch and yaw for a complete "invert switch" + actuatorDesired.Pitch = -actuatorDesired.Pitch; + actuatorDesired.Yaw = -actuatorDesired.Yaw; + } + } + } + + // also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors + if (actuatorDesired.Throttle > cruise_control_min_throttle) + { + // quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8 + // CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7 + actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust; + if (actuatorDesired.Throttle > cruise_control_max_throttle) { + actuatorDesired.Throttle = cruise_control_max_throttle; + } else if (actuatorDesired.Throttle < cruise_control_min_throttle) { + actuatorDesired.Throttle = cruise_control_min_throttle; + } + } + } + if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); } else { @@ -638,9 +713,9 @@ static void ZeroPids(void) static float bound(float val, float range) { if (val < -range) { - val = -range; + return -range; } else if (val > range) { - val = range; + return range; } return val; } @@ -687,11 +762,11 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) StabilizationBankGet(&oldBank); - if (flight_mode < 0) { + if (cur_flight_mode < 0 || cur_flight_mode >= NUM_FMS_POSITIONS) { return; } - switch (cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode]) { + switch (settings.FlightModeMap[cur_flight_mode]) { case 0: StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank); break; @@ -856,10 +931,23 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) vbar_decay = expf(-fakeDt / settings.VbarTau); // force flight mode update - flight_mode = -1; + cur_flight_mode = -1; // Rattitude flight mode anti-windup factor rattitude_anti_windup = settings.RattitudeAntiWindup; + + cruise_control_min_throttle = (float) settings.CruiseControlMinThrottle / 100.0f; + cruise_control_max_throttle = (float) settings.CruiseControlMaxThrottle / 100.0f; + cruise_control_max_angle = settings.CruiseControlMaxAngle; + cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor; + cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f; + cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch; + cruise_control_neutral_thrust = (float) settings.CruiseControlNeutralThrust / 100.0f; + + memcpy( + cruise_control_flight_mode_switch_pos_enable, + settings.CruiseControlFlightModeSwitchPosEnable, + sizeof(cruise_control_flight_mode_switch_pos_enable)); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 873cee58f..6b6ff816b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -144,10 +144,6 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); - addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs1, "Stabilized1", 1, true); - addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs2, "Stabilized2", 1, true); - addWidgetBinding("StabilizationSettings", "FlightModeMap", ui->pidBankSs3, "Stabilized3", 1, true); - addWidgetBinding("ManualControlSettings", "Arming", ui->armControl); addWidgetBinding("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider())); @@ -156,6 +152,8 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addWidget(ui->configurationWizard); addWidget(ui->runCalibration); + autoLoadWidgets(); + populateWidgets(); refreshWidgetsValues(); // Connect the help button @@ -1314,21 +1312,33 @@ void ConfigInputWidget::updatePositionSlider() default: case 6: ui->fmsModePos6->setEnabled(true); + ui->cc_box_5->setEnabled(true); + ui->pidBankSs1_5->setEnabled(true); // pass through case 5: ui->fmsModePos5->setEnabled(true); + ui->cc_box_4->setEnabled(true); + ui->pidBankSs1_4->setEnabled(true); // pass through case 4: ui->fmsModePos4->setEnabled(true); + ui->cc_box_3->setEnabled(true); + ui->pidBankSs1_3->setEnabled(true); // pass through case 3: ui->fmsModePos3->setEnabled(true); + ui->cc_box_2->setEnabled(true); + ui->pidBankSs1_2->setEnabled(true); // pass through case 2: ui->fmsModePos2->setEnabled(true); + ui->cc_box_1->setEnabled(true); + ui->pidBankSs1_1->setEnabled(true); // pass through case 1: ui->fmsModePos1->setEnabled(true); + ui->cc_box_0->setEnabled(true); + ui->pidBankSs1_0->setEnabled(true); // pass through case 0: break; @@ -1337,21 +1347,33 @@ void ConfigInputWidget::updatePositionSlider() switch (manualSettingsDataPriv.FlightModeNumber) { case 0: ui->fmsModePos1->setEnabled(false); + ui->cc_box_0->setEnabled(false); + ui->pidBankSs1_0->setEnabled(false); // pass through case 1: ui->fmsModePos2->setEnabled(false); + ui->cc_box_1->setEnabled(false); + ui->pidBankSs1_1->setEnabled(false); // pass through case 2: ui->fmsModePos3->setEnabled(false); + ui->cc_box_2->setEnabled(false); + ui->pidBankSs1_2->setEnabled(false); // pass through case 3: ui->fmsModePos4->setEnabled(false); + ui->cc_box_3->setEnabled(false); + ui->pidBankSs1_3->setEnabled(false); // pass through case 4: ui->fmsModePos5->setEnabled(false); + ui->cc_box_4->setEnabled(false); + ui->pidBankSs1_4->setEnabled(false); // pass through case 5: ui->fmsModePos6->setEnabled(false); + ui->cc_box_5->setEnabled(false); + ui->pidBankSs1_5->setEnabled(false); // pass through case 6: default: diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 1049a15a9..2fdf932e3 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -6,8 +6,8 @@ 0 0 - 880 - 672 + 796 + 828 @@ -128,8 +128,8 @@ 0 0 - 850 - 589 + 768 + 742 @@ -277,6 +277,13 @@ + + + + Qt::Horizontal + + + @@ -552,11 +559,11 @@ 0 0 - 850 - 589 + 768 + 742 - + 12 @@ -569,420 +576,21 @@ 12 - - - + + + + Qt::Vertical + + + QSizePolicy::Expanding + + - 0 - 230 + 20 + 20 - - - 16777215 - 230 - - - - FlightMode Switch Positions - - - - 12 - - - 12 - - - 12 - - - 12 - - - - - - - - 0 - 0 - - - - - 0 - 16 - - - - - 16777215 - 16 - - - - Pos. 1 - - - - - - - - 0 - 0 - - - - - 16 - 16 - - - - - 16777215 - 16 - - - - Pos. 2 - - - - - - - - 0 - 0 - - - - - 0 - 16 - - - - - 16777215 - 16 - - - - Pos. 3 - - - - - - - - 0 - 0 - - - - - 0 - 16 - - - - - 16777215 - 16 - - - - Pos. 4 - - - - - - - - 0 - 0 - - - - - 0 - 16 - - - - - 16777215 - 16 - - - - Pos. 5 - - - - - - - - 0 - 0 - - - - - 0 - 16 - - - - - 16777215 - 16 - - - - Pos. 6 - - - - - - - - - false - - - - 16777215 - 160 - - - - Qt::StrongFocus - - - This slider moves when you move the flight mode switch -on your remote. It shows currently active flight mode. - -Setup the flight mode channel on the RC Input tab if you have not done so already. - - - 0 - - - 5 - - - 10 - - - 0 - - - 0 - - - Qt::Vertical - - - true - - - QSlider::TicksBelow - - - 1 - - - - - - - 3 - - - - - Qt::StrongFocus - - - Select the stabilization mode on this position (manual/stabilized/auto) - - - - - - - Qt::StrongFocus - - - - - - - Qt::StrongFocus - - - - - - - false - - - Qt::StrongFocus - - - Select the stabilization mode on this position (manual/stabilized/auto) - - - - - - - false - - - Qt::StrongFocus - - - - - - - false - - - Qt::StrongFocus - - - - - - - - - Qt::Horizontal - - - QSizePolicy::Expanding - - - - 40 - 20 - - - - - - - - - - - - Number of flight modes: - - - - - - - Number of positions your FlightMode switch has. - -Default is 3. - -It will be 2 or 3 for most of setups, but it also can be up to 6. -In that case you have to configure your radio mixers so the whole range -from min to max is split into N equal intervals, and you may set arbitrary -channel value for each flight mode. - - - 1 - - - 6 - - - 3 - - - - - - - - - - 75 - true - - - - Avoid "Manual" for multirotors! - - - true - - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 0 - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 0 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - + @@ -992,34 +600,50 @@ channel value for each flight mode. Stabilization Modes Configuration - + 9 - 12 + 6 - - + + Qt::Horizontal - QSizePolicy::Minimum + QSizePolicy::Fixed - 5 + 10 20 - + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + - 0 + 120 20 @@ -1044,120 +668,11 @@ margin:1px; - - - - Qt::Horizontal - - - QSizePolicy::MinimumExpanding - - - - 20 - 20 - - - - - - - - Qt::StrongFocus - - - - - - - Stabilized 1 - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - Qt::Horizontal - - - QSizePolicy::MinimumExpanding - - - - 20 - 20 - - - - - - - - Stabilized 2 - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - Qt::StrongFocus - - - - - - - Qt::StrongFocus - - - - - - - Qt::StrongFocus - - - - - - - - 0 - 20 - - - - - 16777215 - 20 - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -font: bold 12px; -margin:1px; - - - Roll - - - Qt::AlignCenter - - - - + - 0 + 120 20 @@ -1182,113 +697,315 @@ margin:1px; - - - - Qt::StrongFocus - - - - - - - - 102 - 0 - - - - Qt::StrongFocus - - - - - - - Qt::StrongFocus - - - - - - - - 102 - 0 - - - - Qt::StrongFocus - - - - - - - Stabilized 3 - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - 102 - 0 - - - - Qt::StrongFocus - - - - - - - - 102 - 0 - - - - Qt::StrongFocus - - - - - - - - 102 - 0 - - - - Qt::StrongFocus - - - - - + + Qt::Horizontal + + QSizePolicy::Preferred + - 40 + 170 20 - + + + + + 120 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +font: bold 12px; +margin:1px; + + + Roll + + + Qt::AlignCenter + + + + + + + + 106 + 0 + + + + + 105 + 16777215 + + + + QFrame::NoFrame + + + QFrame::Raised + + + + + + Stabilized 1 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + Stabilized 2 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + Stabilized 3 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 0 + 0 + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 0 + 0 + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 0 + 0 + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + Qt::StrongFocus + + + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + + + + + 0 + 230 + + + + + 16777215 + 230 + + + + Flight Mode Switch Positions + + + + 9 + + + 9 + + + 9 + + + 9 + + + 6 + + - 0 + 80 20 @@ -1313,54 +1030,1059 @@ margin:1px; - - + + - 102 - 0 + 132 + 20 - - Qt::StrongFocus + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +font: bold 12px; +margin:1px; + + + Flight Mode Count + + + Qt::AlignCenter - - + + + + + 151 + 16777215 + + + + Number of positions your FlightMode switch has. + +Default is 3. + +It will be 2 or 3 for most of setups, but it also can be up to 6. +In that case you have to configure your radio mixers so the whole range +from min to max is split into N equal intervals, and you may set arbitrary +channel value for each flight mode. + + + 1 + + + 6 + + + 3 + + + + + + + + 0 + 0 + + + + + 75 + true + + + + Avoid "Manual" for multirotors! + + + Qt::AlignCenter + + + true + + + + + Qt::Horizontal - QSizePolicy::MinimumExpanding + QSizePolicy::Fixed - 20 + 10 20 + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + + 105 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +font: bold 12px; +margin:1px; + + + Cruise Control + + + Qt::AlignCenter + + + + + + + Qt::Vertical + + + + + + + + 138 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +font: bold 12px; +margin:1px; + + + Flight Mode + + + Qt::AlignCenter + + + + + + + + 30 + 0 + + + + + 16777215 + 16777215 + + + + + 6 + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:0 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:1 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:2 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:3 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:4 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 0 + 20 + + + + + 16777215 + 20 + + + + + 75 + true + + + + <html><head/><body><p>Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.</p></body></html> + + + Qt::RightToLeft + + + + + + + objname:StabilizationSettings + fieldname:CruiseControlFlightModeSwitchPosEnable + index:5 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + + + + 90 + 0 + + + + + 90 + 16777215 + + + + QFrame::NoFrame + + + QFrame::Raised + + + + 1 + + + 1 + + + 1 + + + 1 + + + 10 + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 16 + + + + Pos. 5 + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 16 + + + + Pos. 3 + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 16 + + + + Pos. 4 + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 16 + + + + Pos. 1 + + + + + + + + 0 + 0 + + + + + 16 + 20 + + + + + 16777215 + 16 + + + + Pos. 2 + + + + + + + + 0 + 0 + + + + + 0 + 20 + + + + + 16777215 + 16 + + + + Pos. 6 + + + + + + + false + + + + 16777215 + 150 + + + + Qt::StrongFocus + + + This slider moves when you move the flight mode switch +on your remote. It shows currently active flight mode. + +Setup the flight mode channel on the RC Input tab if you have not done so already. + + + 0 + + + 5 + + + 10 + + + 0 + + + 0 + + + Qt::Vertical + + + true + + + QSlider::TicksBelow + + + 1 + + + + + + + + + + + 120 + 0 + + + + + 16777215 + 16777215 + + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + false + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + false + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + false + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + Qt::StrongFocus + + + Select the stabilization mode on this position (manual/stabilized/auto) + + + + + + + + + + + 75 + 0 + + + + + 16777215 + 16777215 + + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:0 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:1 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:2 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:3 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:4 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Select which set of roll rates / max bank angles / PIDs you want active on this switch position.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeMap + index:5 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + - - - - Qt::Vertical - - - QSizePolicy::Expanding - - - - 20 - 20 - - - - @@ -1461,8 +2183,8 @@ margin:1px; 0 0 - 850 - 589 + 504 + 156 @@ -1722,15 +2444,6 @@ Applies and Saves all settings to SD - fmsSsPos1Roll - fmsSsPos1Pitch - fmsSsPos1Yaw - fmsSsPos2Roll - fmsSsPos2Pitch - fmsSsPos2Yaw - fmsSsPos3Roll - fmsSsPos3Pitch - fmsSsPos3Yaw tabWidget deadband graphicsView diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index a9447b92f..b3dc4ffe5 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -26972,7 +26972,7 @@ border-radius: 5; - + 0 @@ -27000,6 +27000,9 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + true + 0 @@ -27081,6 +27084,566 @@ border-radius: 5; + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 20 + + + + + + + + Cruise Control + + + + 9 + + + 9 + + + 9 + + + 9 + + + + + + 0 + + + 9 + + + 0 + + + 0 + + + + + <html><head/><body><p>-1, 0, or 1. Cruise Control multiplies the throttle stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrottle) when inverted. 1 says to use the unboosted throttle stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the throttle stick when inverted. +</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + -1.000000000000000 + + + 1.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlInvertedPowerSwitch + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + <html><head/><body><p>This is the bank angle that CruiseControl goes into inverted / power disabled mode. The power for inverted mode is controlled by CruiseControlInvertedPowerSwitch</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + 180.000000000000000 + + + 105.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlMaxAngle + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + <html><head/><body><p>Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle stick is requesting.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 2 + + + 0.000000000000000 + + + 50.000000000000000 + + + 0.250000000000000 + + + 3.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlMaxPowerFactor + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + PowerTrim + + + Qt::AlignCenter + + + + + + + <html><head/><body><p>This needs to be 0 for all copters except CP helis that are using idle up.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + + objname:StabilizationSettings + fieldname:CruiseControlNeutralThrust + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + <html><head/><body><p>If you find that banging the stick around a lot makes the copter climb a bit, adjust this number down a little.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 2 + + + 80.000000000000000 + + + 120.000000000000000 + + + 0.250000000000000 + + + 100.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlPowerTrim + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + MaxPowerFactor + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + MaxAngle + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + MinThrottle + + + Qt::AlignCenter + + + + + + + <html><head/><body><p>Throttle stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for throttle when the copter is inverted. For safety, never set this so low that the trimmed throttle stick cannot get below it.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + 5.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlMinThrottle + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + NeutralThrust + + + Qt::AlignCenter + + + + + + + <html><head/><body><p>Multi-copters should probably use 90% to 95% to leave some headroom for stabilization. CP helis can set this to 100%.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + 0 + + + 90.000000000000000 + + + + objname:StabilizationSettings + fieldname:CruiseControlMaxThrottle + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + MaxThrottle + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 140 + 16 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + InvertedPower + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 90 + 11 + + + + + + + + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:16 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index bc236af75..2b73378b3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -276,12 +276,6 @@ void ConfigTaskWidget::onAutopilotConnect() } invalidateObjects(); m_isConnected = true; - foreach(WidgetBinding * binding, m_widgetBindingsPerObject) { - if (!binding->isEnabled()) { - continue; - } - loadWidgetLimits(binding->widget(), binding->field(), binding->index(), binding->isLimited(), binding->scale()); - } setDirty(false); enableControls(true); refreshWidgetsValues(); @@ -556,7 +550,7 @@ bool ConfigTaskWidget::addShadowWidgetBinding(QString objectName, QString fieldN if (defaultReloadGroups) { addWidgetToReloadGroups(widget, defaultReloadGroups); } - if (!binding->isEnabled()) { + if (binding->isEnabled()) { loadWidgetLimits(widget, binding->field(), binding->index(), isLimited, scale); } return true; @@ -575,20 +569,23 @@ void ConfigTaskWidget::autoLoadWidgets() if (info.isValid()) { bindingStruct uiRelation; - uiRelation.buttonType = none; + uiRelation.buttonType = none; uiRelation.scale = 1; - uiRelation.element = QString(); - uiRelation.haslimits = false; + uiRelation.index = -1; + uiRelation.elementName = QString(); + uiRelation.haslimits = false; foreach(QString str, info.toStringList()) { QString prop = str.split(":").at(0); QString value = str.split(":").at(1); if (prop == "objname") { - uiRelation.objname = value; + uiRelation.objectName = value; } else if (prop == "fieldname") { - uiRelation.fieldname = value; + uiRelation.fieldName = value; } else if (prop == "element") { - uiRelation.element = value; + uiRelation.elementName = value; + } else if (prop == "index") { + uiRelation.index = value.toInt(); } else if (prop == "scale") { if (value == "null") { uiRelation.scale = 1; @@ -661,7 +658,11 @@ void ConfigTaskWidget::autoLoadWidgets() } else { QWidget *wid = qobject_cast(widget); if (wid) { - addWidgetBinding(uiRelation.objname, uiRelation.fieldname, wid, uiRelation.element, uiRelation.scale, uiRelation.haslimits, &uiRelation.buttonGroup); + if (uiRelation.index != -1) { + addWidgetBinding(uiRelation.objectName, uiRelation.fieldName, wid, uiRelation.index, uiRelation.scale, uiRelation.haslimits, &uiRelation.buttonGroup); + } else { + addWidgetBinding(uiRelation.objectName, uiRelation.fieldName, wid, uiRelation.elementName, uiRelation.scale, uiRelation.haslimits, &uiRelation.buttonGroup); + } } } } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h index 14db4b4c2..9ac0959fb 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h @@ -191,9 +191,10 @@ private: enum buttonTypeEnum { none, save_button, apply_button, reload_button, default_button, help_button }; struct bindingStruct { - QString objname; - QString fieldname; - QString element; + QString objectName; + QString fieldName; + QString elementName; + int index; QString url; buttonTypeEnum buttonType; QList buttonGroup; diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index d8cc27017..27128ddba 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -8,6 +8,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 3ac8eb01e..a9334bf1b 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -3,11 +3,11 @@ PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - + + @@ -30,6 +30,15 @@ + + + + + + + + +