From 148b453aad3d7208b0d28ff5be51054f970c27f0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Mar 2014 01:37:22 +0100 Subject: [PATCH] OP-1216 added new mixer output type "reversable motor" to have engines which can can backwards go to a safe level when disarmed --- flight/modules/Actuator/actuator.c | 29 +++++++++++++++---- .../cfg_vehicletypes/configcustomwidget.cpp | 2 ++ .../configgroundvehiclewidget.cpp | 10 +++---- .../config/cfg_vehicletypes/vehicleconfig.cpp | 4 +-- .../config/cfg_vehicletypes/vehicleconfig.h | 25 ++++++++-------- shared/uavobjectdefinition/mixersettings.xml | 1 + 6 files changed, 46 insertions(+), 25 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index dc355a0bd..f8e354ddd 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -283,8 +283,14 @@ static void actuatorTask(__attribute__((unused)) void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool positiveThrottle = throttleDesired >= 0.00f; - bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + bool activeThrottle; + // note: throttle==0 is an inactive throttle and turns motors off + if (allowReverseThrottle == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_TRUE) { + activeThrottle = throttleDesired > 0.00f || throttleDesired < 0.00f; + } else { + activeThrottle = throttleDesired > 0.00f; + } + bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); @@ -338,7 +344,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) continue; } - if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { + if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); } else { status[ct] = -1; @@ -348,18 +354,28 @@ static void actuatorTask(__attribute__((unused)) void *parameters) if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If not armed or motors aren't meant to spin all the time if (!armed || - (!spinWhileArmed && !positiveThrottle)) { + (!spinWhileArmed && !activeThrottle)) { filterAccumulator[ct] = 0; lastResult[ct] = 0; status[ct] = -1; // force min throttle } // If armed meant to keep spinning, - else if ((spinWhileArmed && !positiveThrottle) || + else if ((spinWhileArmed && !activeThrottle) || (status[ct] < 0)) { status[ct] = 0; } } + // Reversable Motors are like Motors but go to neutral instead of minimum + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { + // If not armed or motor is inactive - no "spinwhilearmed" for this engine type + if (!armed || !activeThrottle) { + filterAccumulator[ct] = 0; + lastResult[ct] = 0; + status[ct] = 0; // force neutral throttle + } + } + // If an accessory channel is selected for direct bypass mode // In this configuration the accessory channel is scaled and mapped // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING @@ -462,6 +478,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2, (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); + // note: no feedforward for reversable motors yet for safety reasons if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { if (result < 0.0f) { // idle throttle result = 0.0f; @@ -580,7 +597,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { Channel[n] = actuatorSettings->ChannelMin[n]; - } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) { + } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { Channel[n] = actuatorSettings->ChannelNeutral[n]; } else { Channel[n] = 0; diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp index 05c0a4b1f..7b2abd7df 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.cpp @@ -181,6 +181,8 @@ QString ConfigCustomWidget::updateConfigObjectsFromWidgets() setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); } else if (q->currentText() == "Motor") { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); + } else if (q->currentText() == "ReversableMotor") { + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); } else if (q->currentText() == "Servo") { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); } else if (q->currentText() == "CameraRoll") { diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp index e050f0731..4270fc347 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.cpp @@ -321,7 +321,7 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleMotorcycle(QString airframeTyp // motor int channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); @@ -371,13 +371,13 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleDifferential(QString airframeT // left motor int channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); // right motor channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); @@ -424,11 +424,11 @@ bool ConfigGroundVehicleWidget::setupGroundVehicleCar(QString airframeType) setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); channel = m_aircraft->gvMotor1ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); channel = m_aircraft->gvMotor2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_REVERSABLEMOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE2, 127); // Output success message diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index 545d81d75..1ebfba307 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -42,7 +42,7 @@ VehicleConfig::VehicleConfig(QWidget *parent) : ConfigTaskWidget(parent) channelNames << QString("Channel%1").arg(i + 1); } - mixerTypeDescriptions << "Disabled" << "Motor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw" + mixerTypeDescriptions << "Disabled" << "Motor" << "ReversableMotor" << "Servo" << "CameraRoll" << "CameraPitch" << "CameraYaw" << "Accessory0" << "Accessory1" << "Accessory2" << "Accessory3" << "Accessory4" << "Accessory5"; // This is needed because new style tries to compact things as much as possible in grid @@ -224,7 +224,7 @@ void VehicleConfig::resetMotorAndServoMixers(UAVDataObject *mixer) { for (int channel = 0; channel < (int)VehicleConfig::CHANNEL_NUMELEM; channel++) { QString type = getMixerType(mixer, channel); - if ((type == "Disabled") || (type == "Motor") || (type == "Servo")) { + if ((type == "Disabled") || (type == "Motor") || (type == "ReversableMotor") || (type == "Servo")) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_DISABLED); resetMixerVector(mixer, channel); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h index b767bc081..26c468601 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h @@ -121,18 +121,19 @@ public: /* Enumeration options for field MixerType */ typedef enum { - MIXERTYPE_DISABLED = 0, - MIXERTYPE_MOTOR = 1, - MIXERTYPE_SERVO = 2, - MIXERTYPE_CAMERAROLL = 3, - MIXERTYPE_CAMERAPITCH = 4, - MIXERTYPE_CAMERAYAW = 5, - MIXERTYPE_ACCESSORY0 = 6, - MIXERTYPE_ACCESSORY1 = 7, - MIXERTYPE_ACCESSORY2 = 8, - MIXERTYPE_ACCESSORY3 = 9, - MIXERTYPE_ACCESSORY4 = 10, - MIXERTYPE_ACCESSORY5 = 11 + MIXERTYPE_DISABLED = 0, + MIXERTYPE_MOTOR = 1, + MIXERTYPE_REVERSABLEMOTOR = 2, + MIXERTYPE_SERVO = 3, + MIXERTYPE_CAMERAROLL = 4, + MIXERTYPE_CAMERAPITCH = 5, + MIXERTYPE_CAMERAYAW = 6, + MIXERTYPE_ACCESSORY0 = 7, + MIXERTYPE_ACCESSORY1 = 8, + MIXERTYPE_ACCESSORY2 = 9, + MIXERTYPE_ACCESSORY3 = 10, + MIXERTYPE_ACCESSORY4 = 11, + MIXERTYPE_ACCESSORY5 = 12 } MixerTypeElem; /* Array element names for field MixerVector */ diff --git a/shared/uavobjectdefinition/mixersettings.xml b/shared/uavobjectdefinition/mixersettings.xml index 2a0021b2a..16143bf2d 100644 --- a/shared/uavobjectdefinition/mixersettings.xml +++ b/shared/uavobjectdefinition/mixersettings.xml @@ -26,6 +26,7 @@ +