From 8fef89d55c55f1fe645703b01ade070d1bbb99b2 Mon Sep 17 00:00:00 2001 From: Nuno Guedes Date: Sat, 20 Apr 2013 16:48:23 +0100 Subject: [PATCH 1/4] OP-596 - GCSController output Fix The GCSController widget now outputs the correct throttle range (0..1) on the ManualControlCommand.Throttle. (Also, some empty lines cleaning.) --- .../src/plugins/gcscontrol/gcscontrolgadget.cpp | 9 +++++++-- .../src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp | 4 ---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 54447b84b..17b75dd53 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -75,8 +75,6 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config) // control_sock->close(); control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress); - - controlsMode = GCSControlConfig->getControlsMode(); int i; @@ -102,6 +100,10 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { double pitch = obj->getField("Pitch")->getDouble(); double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); + + if (throttle >= -1.0 && throttle <= 1.0) // necessary against incorect values from not configured joysticks + throttle = -1.0 + (throttle * 2.0); // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) + // Remap RPYT to left X/Y and right X/Y depending on mode switch (controlsMode) { case 1: @@ -187,6 +189,9 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) return; + if (newThrottle != oldThrottle) + newThrottle = (newThrottle+1.0)/2.0; // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); if (buttonPitchControl==0)obj->getField("Pitch")->setDouble(newPitch); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 82a5f7b73..bbafda25e 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -70,9 +70,6 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent) // Connect object updated event from UAVObject to also update check boxes and dropdown connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*))); - - - leftX = 0; leftY = 0; rightX = 0; @@ -129,7 +126,6 @@ void GCSControlGadgetWidget::toggleControl(int state) UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE); mdata.gcsTelemetryUpdatePeriod = 100; m_gcscontrol->checkBoxUDPControl->setEnabled(true); - } else { From 7bfc0f7962c41273ac10ad8ba1c16ae8bb0c6c53 Mon Sep 17 00:00:00 2001 From: Nuno Guedes Date: Sat, 20 Apr 2013 18:31:35 +0100 Subject: [PATCH 2/4] Code style fixes Had forgotten about the code style standards. I hope it's more correct now. --- .../src/plugins/gcscontrol/gcscontrolgadget.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 17b75dd53..c1e65da2a 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -101,8 +101,12 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); - if (throttle >= -1.0 && throttle <= 1.0) // necessary against incorect values from not configured joysticks - throttle = -1.0 + (throttle * 2.0); // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) + // necessary against incorect values from not configured joysticks + if (throttle >= -1.0 && throttle <= 1.0) + { + // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) + throttle = -1.0 + (throttle * 2.0); + } // Remap RPYT to left X/Y and right X/Y depending on mode switch (controlsMode) { @@ -190,7 +194,11 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r return; if (newThrottle != oldThrottle) - newThrottle = (newThrottle+1.0)/2.0; // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + { + // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) + newThrottle = (newThrottle + 1.0) / 2.0; + } + if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll); From 764412fd13a3596556cc6031a2f18f186c673a0a Mon Sep 17 00:00:00 2001 From: Nuno Guedes Date: Wed, 1 May 2013 23:22:41 +0100 Subject: [PATCH 3/4] A few safety parameters added - added a small safety margin to keep the motors from spinning at 0% throttle - added protection against weird values and values outside the margins - some code style fixes added --- .../plugins/gcscontrol/gcscontrolgadget.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index c1e65da2a..b8cea0d8c 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -101,11 +101,17 @@ void GCSControlGadget::manualControlCommandUpdated(UAVObject * obj) { double yaw = obj->getField("Yaw")->getDouble(); double throttle = obj->getField("Throttle")->getDouble(); - // necessary against incorect values from not configured joysticks - if (throttle >= -1.0 && throttle <= 1.0) - { + // necessary against having the wrong joystick profile chosen, which shows weird values + if (throttle > -1.0 && throttle <= 1.0) { // convert ManualControlCommand.Throttle range (0..1) to the widget's throttle stick range (-1..+1) throttle = -1.0 + (throttle * 2.0); + } else { + // with the safety value (line 206), this helps keep the sticks insde the margins + if (throttle <= -1.0) { + throttle = -1.0; + } else { + throttle = 1.0; + } } // Remap RPYT to left X/Y and right X/Y depending on mode @@ -193,11 +199,15 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl()) return; - if (newThrottle != oldThrottle) - { + //if (newThrottle != oldThrottle) { // convert widget's throttle stick range (-1..+1) to ManualControlCommand.Throttle range (0..1) newThrottle = (newThrottle + 1.0) / 2.0; - } + + // safety value to stop the motors from spinning at 0% throttle + if (newThrottle <= 0.01 ) { + newThrottle = -1; + } + //} if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) { From 7f273bd9e681e0c763d8c93201e80f6a9d0c9b8b Mon Sep 17 00:00:00 2001 From: "Richard Flay (Hyper)" Date: Sat, 4 May 2013 11:30:39 +0930 Subject: [PATCH 4/4] Fixes recent, but broken, commit of fix for OP-754. Bunny is bad. --- flight/modules/Attitude/attitude.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 6f4d348f5..06b9a42c2 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -237,7 +237,7 @@ static void AttitudeTask(void *parameters) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); - rollPitchBiasRate = 0.0f + rollPitchBiasRate = 0.0f; if (accel_alpha > 0.0f) accel_filter_enabled = true; init = 1; @@ -437,8 +437,8 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) } // Force the roll & pitch gyro rates to average to zero during initialisation - gyro_correct_int[0] += - gyros->x * rollPitchBiasRate; - gyro_correct_int[1] += - gyros->y * rollPitchBiasRate; + gyro_correct_int[0] += - gyrosData->x * rollPitchBiasRate; + gyro_correct_int[1] += - gyrosData->y * rollPitchBiasRate; // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly)